void Stippler::createVoronoiDiagram() {
VoronoiDiagramGenerator generator;
generator.generateVoronoi( vertsX, vertsY, parameters.points,
0.0f, (float)(image.getWidth() - 1), 0.0f, (float)(image.getHeight() - 1) );
edges.clear();
Point< float > p1, p2;
Edge< float > edge;
generator.resetIterator();
while ( generator.getNext(
edge.begin.x, edge.begin.y, edge.end.x, edge.end.y,
p1.x, p1.y, p2.x, p2.y ) ) {
if ( edge.begin == edge.end ) {
continue;
}
if ( edges.find( p1 ) == edges.end() ) {
edges[p1] = EdgeList();
}
if ( edges.find( p2 ) == edges.end() ) {
edges[p2] = EdgeList();
}
edges[p1].push_back( edge );
edges[p2].push_back( edge );
}
}
int main(int argc,char **argv)
{
float xValues[4] = {-22, -17, 4,22};
float yValues[4] = {-9, 31,13,-5};
long count = 4;
VoronoiDiagramGenerator vdg;
vdg.generateVoronoi(xValues,yValues,count, -100,100,-100,100,3);
vdg.resetIterator();
float x1,y1,x2,y2;
printf("\n-------------------------------\n");
while(vdg.getNext(x1,y1,x2,y2))
{
printf("GOT Line (%f,%f)->(%f,%f)\n",x1,y1,x2, y2);
}
return 0;
}
int main(int argc, char** argv) {
if(argc<2){
cout << "Configuration file is not specified." << endl ;
return -1;
}
Configuration* config = new Configuration(argv[1]);
Plotter* plot = new Plotter(gnuPlot, config);
cout << "generating voronoi diagram..." << endl ;
float* xValues = new float[100];
float* yValues = new float[100];
long count = 100;
for(int i=0;i<count;i++){
xValues[i] = utility::unifRand(-10, 10);
yValues[i] = utility::unifRand(-10, 10);
}
VoronoiDiagramGenerator vdg;
vdg.generateVoronoi(xValues,yValues,count, -10,10,-10,10,0);
cout << "here it comes..." << endl ;
vdg.resetIterator();
float x1,y1,x2,y2;
printf("\n-------------------------------\n");
while(vdg.getNext(x1,y1,x2,y2))
{
plot->drawLine(x1,y1,x2, y2);
printf("GOT Line (%f,%f)->(%f,%f)\n",x1,y1,x2, y2);
}
plot->close();
delete config;
delete plot;
return 0;
}
int main(int argc,char **argv)
{
auto data = ReadFile<double>(std::string(argv[1]));
float xValues[data.size()];
float yValues[data.size()];
for(size_t i = 0; i < data.size(); ++i){
xValues[i] = data[i][0];
yValues[i] = data[i][1];
}
long count = data.size();
float max_x = *std::max_element(xValues, xValues + count);
float max_y = *std::max_element(yValues, yValues + count);
float min_x = *std::min_element(xValues, xValues + count);
float min_y = *std::min_element(yValues, yValues + count);
VoronoiDiagramGenerator vdg;
vdg.generateVoronoi(xValues,yValues,count, min_x, max_x, min_y, max_y, 0);
vdg.resetIterator();
float x1,y1,x2,y2;
while(vdg.getNext(x1,y1,x2,y2))
{
printf("%f %f \n",x1,y1);
printf("%f %f \n",x2, y2);
printf("\n");
}
return 0;
}
static PyObject* getMesh(int npoints, double *x, double *y)
{
PyObject *vertices = NULL, *edge_db = NULL, *tri_edges = NULL, *tri_nbrs = NULL;
PyObject *temp;
int tri0, tri1, reg0, reg1;
double tri0x, tri0y, tri1x, tri1y;
int length, numtri, i, j;
intp dim[MAX_DIMS];
int *edge_db_ptr, *tri_edges_ptr, *tri_nbrs_ptr;
double *vertices_ptr;
VoronoiDiagramGenerator vdg;
vdg.generateVoronoi(x, y, npoints, -100, 100, -100, 100, 0);
vdg.getNumbers(length, numtri);
// Count the actual number of edges
i = 0;
vdg.resetEdgeListIter();
while (vdg.getNextDelaunay(tri0, tri0x, tri0y, tri1, tri1x, tri1y, reg0, reg1))
i++;
length = i;
dim[0] = length;
dim[1] = 2;
edge_db = PyArray_SimpleNew(2, dim, NPY_INT);
if (!edge_db) goto fail;
edge_db_ptr = (int*)PyArray_DATA((PyArrayObject*)edge_db);
dim[0] = numtri;
vertices = PyArray_SimpleNew(2, dim, NPY_DOUBLE);
if (!vertices) goto fail;
vertices_ptr = (double*)PyArray_DATA((PyArrayObject*)vertices);
dim[1] = 3;
tri_edges = PyArray_SimpleNew(2, dim, NPY_INT);
if (!tri_edges) goto fail;
tri_edges_ptr = (int*)PyArray_DATA((PyArrayObject*)tri_edges);
tri_nbrs = PyArray_SimpleNew(2, dim, NPY_INT);
if (!tri_nbrs) goto fail;
tri_nbrs_ptr = (int*)PyArray_DATA((PyArrayObject*)tri_nbrs);
for (i=0; i<(3*numtri); i++) {
tri_edges_ptr[i] = tri_nbrs_ptr[i] = -1;
}
vdg.resetEdgeListIter();
i = -1;
while (vdg.getNextDelaunay(tri0, tri0x, tri0y, tri1, tri1x, tri1y, reg0, reg1)) {
i++;
INDEX2(edge_db_ptr,i,0) = reg0;
INDEX2(edge_db_ptr,i,1) = reg1;
if (tri0 > -1) {
INDEX2(vertices_ptr,tri0,0) = tri0x;
INDEX2(vertices_ptr,tri0,1) = tri0y;
for (j=0; j<3; j++) {
if (INDEX3(tri_edges_ptr,tri0,j) == i) break;
if (INDEX3(tri_edges_ptr,tri0,j) == -1) {
INDEX3(tri_edges_ptr,tri0,j) = i;
INDEX3(tri_nbrs_ptr,tri0,j) = tri1;
break;
}
}
}
if (tri1 > -1) {
INDEX2(vertices_ptr,tri1,0) = tri1x;
INDEX2(vertices_ptr,tri1,1) = tri1y;
for (j=0; j<3; j++) {
if (INDEX3(tri_edges_ptr,tri1,j) == i) break;
if (INDEX3(tri_edges_ptr,tri1,j) == -1) {
INDEX3(tri_edges_ptr,tri1,j) = i;
INDEX3(tri_nbrs_ptr,tri1,j) = tri0;
break;
}
}
}
}
// tri_edges contains lists of edges; convert to lists of nodes in
// counterclockwise order and reorder tri_nbrs to match. Each node
// corresponds to the edge opposite it in the triangle.
reorder_edges(npoints, numtri, x, y, edge_db_ptr, tri_edges_ptr,
tri_nbrs_ptr);
temp = Py_BuildValue("(OOOO)", vertices, edge_db, tri_edges, tri_nbrs);
if (!temp) goto fail;
Py_DECREF(vertices);
Py_DECREF(edge_db);
Py_DECREF(tri_edges);
Py_DECREF(tri_nbrs);
return temp;
fail:
Py_XDECREF(vertices);
Py_XDECREF(edge_db);
Py_XDECREF(tri_edges);
Py_XDECREF(tri_nbrs);
return NULL;
}
// In this function, the Voronoi cell is calculated, integrated. A new goal IS NOT published, as this node should only be used with integration of teleop.
void SimpleDeploymentDummy::publish()
{
if ( got_map_ && got_me_ ) {
double factor = map_.info.resolution / resolution_; // zoom factor for openCV visualization
ROS_DEBUG("SimpleDeployment: Map received, determining Voronoi cells");
removeOldAgents();
// Create variables for x-values, y-values and the maximum and minimum of these, needed for VoronoiDiagramGenerator
float xvalues[agent_catalog_.size()];
float yvalues[agent_catalog_.size()];
double xmin = 0.0, xmax = 0.0, ymin = 0.0, ymax = 0.0;
cv::Point seedPt = cv::Point(1,1);
// Create empty image with the size of the map to draw points and voronoi diagram in
cv::Mat vorImg = cv::Mat::zeros(map_.info.height*factor,map_.info.width*factor,CV_8UC1);
for ( uint i = 0; i < agent_catalog_.size(); i++ ) {
geometry_msgs::Pose pose = agent_catalog_[i].getPose();
// Keep track of x and y values
xvalues[i] = pose.position.x;
yvalues[i] = pose.position.y;
// Keep track of min and max x
if ( pose.position.x < xmin ) {
xmin = pose.position.x;
} else if ( pose.position.x > xmax ) {
xmax = pose.position.x;
}
// Keep track of min and max y
if ( pose.position.y < ymin ) {
ymin = pose.position.y;
} else if ( pose.position.y > ymax ) {
ymax = pose.position.y;
}
// Store point as seed point if it represents this agent
if ( agent_catalog_[i].getName() == this_agent_.getName() ){
// Scale positions in metric system column and row numbers in image
int c = ( pose.position.x - map_.info.origin.position.x ) * factor / map_.info.resolution;
int r = map_.info.height * factor - ( pose.position.y - map_.info.origin.position.y ) * factor / map_.info.resolution;
cv::Point pt = cv::Point(c,r);
seedPt = pt;
}
// Draw point on image
// cv::circle( vorImg, pt, 3, WHITE, -1, 8);
}
ROS_DEBUG("SimpleDeployment: creating a VDG object and generating Voronoi diagram");
// Construct a VoronoiDiagramGenerator (VoronoiDiagramGenerator.h)
VoronoiDiagramGenerator VDG;
xmin = map_.info.origin.position.x; xmax = map_.info.width * map_.info.resolution + map_.info.origin.position.x;
ymin = map_.info.origin.position.y; ymax = map_.info.height * map_.info.resolution + map_.info.origin.position.y;
// Generate the Voronoi diagram using the collected x and y values, the number of points, and the min and max x and y values
VDG.generateVoronoi(xvalues,yvalues,agent_catalog_.size(),float(xmin),float(xmax),float(ymin),float(ymax));
float x1,y1,x2,y2;
ROS_DEBUG("SimpleDeployment: collecting line segments from the VDG object");
// Collect the generated line segments from the VDG object
while(VDG.getNext(x1,y1,x2,y2))
{
// Scale the line segment end-point coordinates to column and row numbers in image
int c1 = ( x1 - map_.info.origin.position.x ) * factor / map_.info.resolution;
int r1 = vorImg.rows - ( y1 - map_.info.origin.position.y ) * factor / map_.info.resolution;
int c2 = ( x2 - map_.info.origin.position.x ) * factor / map_.info.resolution;
int r2 = vorImg.rows - ( y2 - map_.info.origin.position.y ) * factor / map_.info.resolution;
// Draw line segment
cv::Point pt1 = cv::Point(c1,r1),
pt2 = cv::Point(c2,r2);
cv::line(vorImg,pt1,pt2,WHITE);
}
ROS_DEBUG("SimpleDeployment: drawing map occupancygrid and resizing to voronoi image size");
// Create cv image from map data and resize it to the same size as voronoi image
cv::Mat mapImg = drawMap();
cv::Mat viewImg(vorImg.size(),CV_8UC1);
cv::resize(mapImg, viewImg, vorImg.size(), 0.0, 0.0, cv::INTER_NEAREST );
// Add images together to make the total image
cv::Mat totalImg(vorImg.size(),CV_8UC1);
cv::bitwise_or(viewImg,vorImg,totalImg);
cv::Mat celImg = cv::Mat::zeros(vorImg.rows+2, vorImg.cols+2, CV_8UC1);
cv::Scalar newVal = cv::Scalar(1), upDiff = cv::Scalar(100), loDiff = cv::Scalar(256);
cv::Rect rect;
cv::floodFill(totalImg,celImg,seedPt,newVal,&rect,loDiff,upDiff,4 + (255 << 8) + cv::FLOODFILL_MASK_ONLY);
// Compute the center of mass of the Voronoi cell
cv::Moments m = moments(celImg, false);
cv::Point centroid(m.m10/m.m00, m.m01/m.m00);
cv::circle( celImg, centroid, 3, BLACK, -1, 8);
// Convert seed point to celImg coordinate system (totalImg(x,y) = celImg(x+1,y+1)
cv::Point onePt(1,1);
centroid = centroid - onePt;
for ( uint i = 0; i < agent_catalog_.size(); i++ ){
int c = ( xvalues[i] - map_.info.origin.position.x ) * factor / map_.info.resolution;
int r = map_.info.height * factor - ( yvalues[i] - map_.info.origin.position.y ) * factor / map_.info.resolution;
cv::Point pt = cv::Point(c,r);
cv::circle( totalImg, pt, 3, GRAY, -1, 8);
}
cv::circle( totalImg, seedPt, 3, WHITE, -1, 8);
cv::circle( totalImg, centroid, 2, WHITE, -1, 8); //where centroid is the goal position
// Due to bandwidth issues, only display this image if requested
if (show_cells_) {
cv::imshow( "Display window", totalImg );
}
cv::waitKey(3);
}
else {
ROS_DEBUG("SimpleDeployment: No map received");
}
}
int main(int argc, const char * argv[]) {
//variables provided by publisher
const int count = 5; //number of sites(robots)
float xValues[count] = {4, 5, 19, 36, 51}; //X position of sites(robots)
float yValues[count] = {13, 49, 88, 76, 27}; //Y position of sites(robots)
//variables should be provided by "Graham Scan" Algorithm
const int nCzyBdyVert = 8;
float boundaryPos[nCzyBdyVert][2] = {{2, -15}, {35, -23}, {72, -23}, {112, -3}, {109, 98}, {100, 120}, {-5, 120}, {-43, 36}};
//define the minimum and maximum X and Y values for the Fortune's Algorithm
float minX, maxX, minY, maxY;
for (int i=0; i<nCzyBdyVert; i++) {
if (i==0) {
minX = maxX = boundaryPos[i][0];
minY = maxY = boundaryPos[i][1];
}
else {
if (boundaryPos[i][0]<minX) minX = boundaryPos[i][0];
else if(boundaryPos[i][0]>maxX) maxX = boundaryPos[i][0];
if (boundaryPos[i][1]<minY) minY = boundaryPos[i][1];
else if(boundaryPos[i][1]>maxY) maxY = boundaryPos[i][1];
}
}
//cout << "minX: " << minX << ". maxX: " << maxX << ". minY: " << minY << ". maxY: " << maxY << endl;
//Store the position of the sites in a Matrix
int nSites = Matrix_Size(xValues);
Matrix sitesPos(nSites,2);
for(int i=0; i<Matrix_Size(xValues);i++){
sitesPos.setElement(i, 0, xValues[i]); //sitePos.elements[i][0] = xValues[i];
sitesPos.setElement(i, 1, yValues[i]);
}
//sitesPos.printArray("Sites");
int iteration=1;
while (iteration<=100)
{
cout << endl << "Iteration " << iteration;
CentroidGenerator cg;
VoronoiDiagramGenerator vdg;
vdg.generateVoronoi(xValues, yValues, count, minX, maxX, minY, maxY, 3);
vdg.resetIterator();
float x1,y1,x2,y2;
int a=1;
printf("\n-------------------------------\n");
while(vdg.getNext(x1,y1,x2,y2))
{
//printf("GOT Line (%.4f,%.4f)->(%.4f,%.4f)\n", x1,y1,x2,y2);
//printf("v%dx = [%.4f,%.4f];\n", a, x1,x2); //to work with MATLAB
//printf("v%dy = [%.4f,%.4f];\n", a, y1,y2); //to work with MATLAB
a++;
if (x1!=x2 || y1!=y2) //if condition necessary due to some unknown problem (Fortune's Algorithm generating vertices that shouldn't exist)
{
cg.posVertVector.push_back(x1); cg.posVertVector.push_back(y1);
cg.posVertVector.push_back(x2); cg.posVertVector.push_back(y2);
} //even though it seems to not affect the centroids' position
}
cout << endl;
//After store position of all vertices, store the position of the edges of the plane (polygon boundary)
for (int i=0; i<nCzyBdyVert; i++) {
cg.posVertVector.push_back(boundaryPos[i][0]);
cg.posVertVector.push_back(boundaryPos[i][1]);
}
//Return the position of the centroids
sitesPos = cg.generateCentroid(cg.posVertVector, sitesPos, nSites, minX, maxX, minY, maxY, nCzyBdyVert);
//sitesPos.printArray("new");
//Split the Centroid Matrix in X and Y vectors in order to pass in to the Fortune's Algorithm
for (int i=0; i<sitesPos.rows; i++) {
xValues[i]=sitesPos.elements[i][0];
yValues[i]=sitesPos.elements[i][1];
}
iteration++;
}
return 0;
}
void Server::generateVoronoi() {
clock_t start = clock();
for(unsigned long k=0;k<REPCOUNT;k++){
std::vector<Point> sPoints;
std::vector<Point> vPoints;
std::vector<Point> points;
sPoints.clear();
vPoints.clear();
points.clear();
VoronoiDiagramGenerator vdg;
set <Server*>::iterator it;
float x1,y1,x2,y2;
Point curPoint;
double distTp, newDist;
// Get all server locations
points.push_back(this->loc);
for(it = this->neighbours.begin(); it != this->neighbours.end(); it++) {
points.push_back((*it)->loc);
}
int count = points.size();
float xValues[count];
float yValues[count];
vPoints.push_back(Point(0,0));
vPoints.push_back(Point(WIDTH,0));
vPoints.push_back(Point(WIDTH,WIDTH));
vPoints.push_back(Point(0,WIDTH));
for (int i=0;i<count;i++) {
xValues[i] = points.at(i).x();
yValues[i] = points.at(i).y();
}
vdg.generateVoronoi(xValues,yValues,count, 0,WIDTH,0,WIDTH);
vdg.resetIterator();
// printf("\n-------------------------------\n");
while(vdg.getNext(x1,y1,x2,y2))
{
// printf("GOT Line (%g,%g)->(%g,%g)\n",x1,y1,x2, y2);
vPoints.push_back(Point(x1,y1));
vPoints.push_back(Point(x2,y2));
}
vPoints = myUnique(vPoints);
this->deleteCell();
bool mine;
for (unsigned int i=0;i<vPoints.size();i++) {
mine = true;
curPoint = vPoints[i];
distTp = this->loc.dist(curPoint);
for(it = this->neighbours.begin(); it != this->neighbours.end(); it++) {
newDist = (*it)->loc.dist(curPoint);
// if(distTp < newDist){
// mine = true;
// }else if(abs(newDist - distTp) < EPS) {
// mine = true;
// }else if(newDist < distTp){
// mine = false;
// }
if (abs(newDist - distTp) > EPS) {
if (newDist < distTp) {
mine = false;
}
}
}
if (mine) {
sPoints.push_back(curPoint);
}
}
this->GrahamScan(sPoints);
}
clock_t end = clock();
double cpu_time = static_cast<double>( end - start )/REPCOUNT;
printf("generateVoronoi() comp_time = %f \n",cpu_time);
}
