diff --git a/CMakeLists.txt b/CMakeLists.txt index 4d868f8..0468f68 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,6 +57,7 @@ add_executable(rosbuzz_node src/rosbuzz.cpp src/roscontroller.cpp src/buzz_utility.cpp src/buzzuav_closures.cpp + src/VoronoiDiagramGenerator.cpp src/buzz_update.cpp) target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} buzz buzzdbg pthread) add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp) diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index 6f1588b..3a922c7 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -87,7 +87,7 @@ function barrier_wait(threshold, transf, resumef, bc) { function barrier_allgood(barrier, bc) { barriergood = 1 barrier.foreach(function(key, value, robot){ - log("VS entry : ", key, " ", value, " ", robot) + #log("VS entry : ", key, " ", value, " ", robot) if(key == "d"){ if(value == 1) return 1 diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz index 072d312..614e2ea 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/buzz_scripts/include/act/naviguation.bzz @@ -2,24 +2,44 @@ GOTO_MAXVEL = 1.5 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 0.4 # m. GOTOANG_TOL = 0.1 # rad. +GPSlimit = {.1={.lat=45.510386, .lng=-73.610400}, + .2={.lat=45.509839, .lng=-73.610047}, + .3={.lat=45.510859, .lng=-73.608714}, + .4={.lat=45.510327, .lng=-73.608393}} # Core naviguation function to travel to a GPS target location. function goto_gps(transf) { - m_navigation = vec_from_gps(cur_goal.latitude, cur_goal.longitude, 0) - #print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) - if(math.vec2.length(m_navigation)>GOTO_MAXDIST) - log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )") - else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination - transf() - } else { - m_navigation = LimitSpeed(m_navigation, 1.0) - #m_navigation = LCA(m_navigation) - goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0) - } + if(Geofence()) { + m_navigation = vec_from_gps(cur_goal.latitude, cur_goal.longitude, 0) + #print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) + if(math.vec2.length(m_navigation)>GOTO_MAXDIST) + log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )") + else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination + transf() + } else { + m_navigation = LimitSpeed(m_navigation, 1.0) + #m_navigation = LCA(m_navigation) + goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0) + } + } else + log("Geofencing prevents from going to that location!") } function LimitSpeed(vel_vec, factor){ if(math.vec2.length(vel_vec)>GOTO_MAXVEL*factor) vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec)) return vel_vec +} + +function Geofence(){ #TODO: rotate the fence box to really fit the coordinates + if(cur_goal.latitude > GPSlimit[1].lat and cur_goal.latitude > GPSlimit[2].lat and cur_goal.latitude > GPSlimit[3].lat and cur_goal.latitude > GPSlimit[4].lat) + return 0; + if(cur_goal.latitude < GPSlimit[1].lat and cur_goal.latitude < GPSlimit[2].lat and cur_goal.latitude < GPSlimit[3].lat and cur_goal.latitude < GPSlimit[4].lat) + return 0; + if(cur_goal.longitude > GPSlimit[1].lng and cur_goal.longitude > GPSlimit[2].lng and cur_goal.longitude > GPSlimit[3].lng and cur_goal.longitude > GPSlimit[4].lng) + return 0; + if(cur_goal.longitude < GPSlimit[1].lng and cur_goal.longitude < GPSlimit[2].lng and cur_goal.longitude < GPSlimit[3].lng and cur_goal.longitude < GPSlimit[4].lng) + return 0; + + return 1 } \ No newline at end of file diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 2f18024..9df236e 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -100,7 +100,7 @@ function stop() { wpreached = 1 function indiWP() { - BVMSTATE = "INDIWP" + BVMSTATE = "WAYPOINT" check_rc_wp() wpi = v_wp.get(id) @@ -191,12 +191,12 @@ function pursuit() { } # Lennard-Jones interaction magnitude -TARGET = 16.0 +TARGET = 8.0 EPSILON = 30.0 #30 GAIN_ATT = 50.0 GAIN_REP = 30.0 function lj_magnitude(dist, target, epsilon) { - lj = -(epsilon / dist) * ((target / dist)^4)# - (target / dist)^2) #repulse only to avoid each other + lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) #repulse only to avoid each other return lj } @@ -239,6 +239,71 @@ function lennardjones() { goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0) } +# State function that calculates and actuates Voronoi Centroidal tessellation coverage (attractor/repulsors) +counter = 0 +function voronoicentroid() { + BVMSTATE="DEPLOY" + if(counter==0 and id!=0) { + pts = getbounds() + it_pts = 0 + #table_print(pts) + if(neighbors.count() > 0) { + neighbors.foreach(function(rid, data) { + pts[it_pts]=math.vec2.newp(data.distance,data.azimuth+pts.oa) + it_pts = it_pts + 1}) + #table_print(pts) + voronoi(pts) + } + counter=10 + } + counter=counter-1 + + goto_gps(voronoicentroid_done) +} + +function voronoicentroid_done() { + BVMSTATE="DEPLOY" +} + +function getbounds(){ + var RBlimit = {} + foreach(GPSlimit, function(key, value) { + RBlimit[key] = vec_from_gps(value.lat, value.lng, 0) + }) + minY = RBlimit[1].y + minYid = 1 + maxY = RBlimit[1].y + maxYid = 1 + foreach(RBlimit, function(key, value) { + if(value.ymaxY){ + maxY = value.y + maxYid = key + } + }) + minY2 = maxY + minY2id = maxYid + foreach(RBlimit, function(key, value) { + if(value.y math.pi/2.0) + angle_offset = math.pi - angle_offset + #log(angle_offset, minYid, minY2id, maxYid) + dvec = math.vec2.sub(RBlimit[maxYid],RBlimit[minYid]) + new_maxY = math.vec2.add(RBlimit[minYid], math.vec2.newp(math.vec2.length(dvec),math.vec2.angle(dvec)+angle_offset)) + if(new_maxY.x > RBlimit[minYid].x) + return {.miny=minY, .minx=RBlimit[minYid].x, .maxy=new_maxY.y, .maxx=new_maxY.x} + else + return {.miny=minY, .minx=new_maxY.x, .maxy=new_maxY.y, .maxx=RBlimit[minYid].x, .oa=angle_offset} +} + # Custom state function for the developer to play with function cusfun(){ BVMSTATE="CUSFUN" diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 5f2d9c1..d9703f4 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -72,7 +72,7 @@ function step() { statef=launch_switch else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE statef=goinghome - else if(BVMSTATE=="INDIWP") + else if(BVMSTATE=="WAYPOINT") statef=indiWP else if(BVMSTATE=="IDLE") statef=idle @@ -86,6 +86,8 @@ function step() { statef=resetGraph else if(BVMSTATE=="BIDDING") # check the absolute path of the waypointlist csv file in bidding.bzz statef=bidding + else if(BVMSTATE=="DEPLOY") # check the absolute path of the waypointlist csv file in bidding.bzz + statef=voronoicentroid else if(BVMSTATE=="GRAPH_FREE") statef=DoFree else if(BVMSTATE=="GRAPH_ASKING") diff --git a/include/VoronoiDiagramGenerator.h b/include/VoronoiDiagramGenerator.h new file mode 100644 index 0000000..a812c78 --- /dev/null +++ b/include/VoronoiDiagramGenerator.h @@ -0,0 +1,248 @@ +/* +* The author of this software is Steven Fortune. Copyright (c) 1994 by AT&T +* Bell Laboratories. +* Permission to use, copy, modify, and distribute this software for any +* purpose without fee is hereby granted, provided that this entire notice +* is included in all copies of any software which is or includes a copy +* or modification of this software and in all copies of the supporting +* documentation for such software. +* THIS SOFTWARE IS BEING PROVIDED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED +* WARRANTY. IN PARTICULAR, NEITHER THE AUTHORS NOR AT&T MAKE ANY +* REPRESENTATION OR WARRANTY OF ANY KIND CONCERNING THE MERCHANTABILITY +* OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE. +*/ + +/* +* This code was originally written by Stephan Fortune in C code. I, Shane O'Sullivan, +* have since modified it, encapsulating it in a C++ class and, fixing memory leaks and +* adding accessors to the Voronoi Edges. +* Permission to use, copy, modify, and distribute this software for any +* purpose without fee is hereby granted, provided that this entire notice +* is included in all copies of any software which is or includes a copy +* or modification of this software and in all copies of the supporting +* documentation for such software. +* THIS SOFTWARE IS BEING PROVIDED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED +* WARRANTY. IN PARTICULAR, NEITHER THE AUTHORS NOR AT&T MAKE ANY +* REPRESENTATION OR WARRANTY OF ANY KIND CONCERNING THE MERCHANTABILITY +* OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE. +*/ + +#ifndef VORONOI_DIAGRAM_GENERATOR +#define VORONOI_DIAGRAM_GENERATOR + +#include +#include +#include + + +#ifndef NULL +#define NULL 0 +#endif +#define DELETED -2 + +#define le 0 +#define re 1 + + + +struct Freenode +{ + struct Freenode *nextfree; +}; + +struct FreeNodeArrayList +{ + struct Freenode* memory; + struct FreeNodeArrayList* next; + +}; + +struct Freelist +{ + struct Freenode *head; + int nodesize; +}; + +struct Point +{ + float x,y; +}; + +// structure used both for sites and for vertices +struct Site +{ + struct Point coord; + int sitenbr; + int refcnt; +}; + + + +struct Edge +{ + float a,b,c; + struct Site *ep[2]; + struct Site *reg[2]; + int edgenbr; + +}; + +struct GraphEdge +{ + float x1,y1,x2,y2; + struct GraphEdge* next; +}; + + + + +struct Halfedge +{ + struct Halfedge *ELleft, *ELright; + struct Edge *ELedge; + int ELrefcnt; + char ELpm; + struct Site *vertex; + float ystar; + struct Halfedge *PQnext; +}; + + + + +class VoronoiDiagramGenerator +{ +public: + VoronoiDiagramGenerator(); + ~VoronoiDiagramGenerator(); + + bool generateVoronoi(float *xValues, float *yValues, int numPoints, float minX, float maxX, float minY, float maxY, float minDist=0); + + void resetIterator() + { + iteratorEdges = allEdges; + } + + bool getNext(float& x1, float& y1, float& x2, float& y2) + { + if(iteratorEdges == 0) + return false; + + x1 = iteratorEdges->x1; + x2 = iteratorEdges->x2; + y1 = iteratorEdges->y1; + y2 = iteratorEdges->y2; + + iteratorEdges = iteratorEdges->next; + + return true; + } + + +private: + void cleanup(); + void cleanupEdges(); + char *getfree(struct Freelist *fl); + struct Halfedge *PQfind(); + int PQempty(); + + + + struct Halfedge **ELhash; + struct Halfedge *HEcreate(), *ELleft(), *ELright(), *ELleftbnd(); + struct Halfedge *HEcreate(struct Edge *e,int pm); + + + struct Point PQ_min(); + struct Halfedge *PQextractmin(); + void freeinit(struct Freelist *fl,int size); + void makefree(struct Freenode *curr,struct Freelist *fl); + void geominit(); + void plotinit(); + bool voronoi(int triangulate); + void ref(struct Site *v); + void deref(struct Site *v); + void endpoint(struct Edge *e,int lr,struct Site * s); + + void ELdelete(struct Halfedge *he); + struct Halfedge *ELleftbnd(struct Point *p); + struct Halfedge *ELright(struct Halfedge *he); + void makevertex(struct Site *v); + void out_triple(struct Site *s1, struct Site *s2,struct Site * s3); + + void PQinsert(struct Halfedge *he,struct Site * v, float offset); + void PQdelete(struct Halfedge *he); + bool ELinitialize(); + void ELinsert(struct Halfedge *lb, struct Halfedge *newHe); + struct Halfedge * ELgethash(int b); + struct Halfedge *ELleft(struct Halfedge *he); + struct Site *leftreg(struct Halfedge *he); + void out_site(struct Site *s); + bool PQinitialize(); + int PQbucket(struct Halfedge *he); + void clip_line(struct Edge *e); + char *myalloc(unsigned n); + int right_of(struct Halfedge *el,struct Point *p); + + struct Site *rightreg(struct Halfedge *he); + struct Edge *bisect(struct Site *s1,struct Site *s2); + float dist(struct Site *s,struct Site *t); + struct Site *intersect(struct Halfedge *el1, struct Halfedge *el2, struct Point *p=0); + + void out_bisector(struct Edge *e); + void out_ep(struct Edge *e); + void out_vertex(struct Site *v); + struct Site *nextone(); + + void pushGraphEdge(float x1, float y1, float x2, float y2); + + void openpl(); + void line(float x1, float y1, float x2, float y2); + void circle(float x, float y, float radius); + void range(float minX, float minY, float maxX, float maxY); + + + struct Freelist hfl; + struct Halfedge *ELleftend, *ELrightend; + int ELhashsize; + + int triangulate, sorted, plot, debug; + float xmin, xmax, ymin, ymax, deltax, deltay; + + struct Site *sites; + int nsites; + int siteidx; + int sqrt_nsites; + int nvertices; + struct Freelist sfl; + struct Site *bottomsite; + + int nedges; + struct Freelist efl; + int PQhashsize; + struct Halfedge *PQhash; + int PQcount; + int PQmin; + + int ntry, totalsearch; + float pxmin, pxmax, pymin, pymax, cradius; + int total_alloc; + + float borderMinX, borderMaxX, borderMinY, borderMaxY; + + FreeNodeArrayList* allMemoryList; + FreeNodeArrayList* currentMemoryBlock; + + GraphEdge* allEdges; + GraphEdge* iteratorEdges; + + float minDistanceBetweenSites; + +}; + +int scomp(const void *p1,const void *p2); + + +#endif + + diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 967f521..11efd3a 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -98,7 +98,7 @@ double* getgoto(); * returns the current grid */ std::map> getgrid(); - +int voronoi_center(buzzvm_t vm); /* * returns the gimbal commands diff --git a/src/VoronoiDiagramGenerator.cpp b/src/VoronoiDiagramGenerator.cpp new file mode 100644 index 0000000..49c1b5b --- /dev/null +++ b/src/VoronoiDiagramGenerator.cpp @@ -0,0 +1,1006 @@ +/* +* The author of this software is Steven Fortune. Copyright (c) 1994 by AT&T +* Bell Laboratories. +* Permission to use, copy, modify, and distribute this software for any +* purpose without fee is hereby granted, provided that this entire notice +* is included in all copies of any software which is or includes a copy +* or modification of this software and in all copies of the supporting +* documentation for such software. +* THIS SOFTWARE IS BEING PROVIDED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED +* WARRANTY. IN PARTICULAR, NEITHER THE AUTHORS NOR AT&T MAKE ANY +* REPRESENTATION OR WARRANTY OF ANY KIND CONCERNING THE MERCHANTABILITY +* OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE. +*/ + +/* +* This code was originally written by Stephan Fortune in C code. I, Shane O'Sullivan, +* have since modified it, encapsulating it in a C++ class and, fixing memory leaks and +* adding accessors to the Voronoi Edges. +* Permission to use, copy, modify, and distribute this software for any +* purpose without fee is hereby granted, provided that this entire notice +* is included in all copies of any software which is or includes a copy +* or modification of this software and in all copies of the supporting +* documentation for such software. +* THIS SOFTWARE IS BEING PROVIDED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED +* WARRANTY. IN PARTICULAR, NEITHER THE AUTHORS NOR AT&T MAKE ANY +* REPRESENTATION OR WARRANTY OF ANY KIND CONCERNING THE MERCHANTABILITY +* OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE. +*/ + +#include "VoronoiDiagramGenerator.h" + +VoronoiDiagramGenerator::VoronoiDiagramGenerator() +{ + siteidx = 0; + sites = 0; + + allMemoryList = new FreeNodeArrayList; + allMemoryList->memory = 0; + allMemoryList->next = 0; + currentMemoryBlock = allMemoryList; + allEdges = 0; + iteratorEdges = 0; + minDistanceBetweenSites = 0; +} + +VoronoiDiagramGenerator::~VoronoiDiagramGenerator() +{ + cleanup(); + cleanupEdges(); + + if(allMemoryList != 0) + delete allMemoryList; +} + + + +bool VoronoiDiagramGenerator::generateVoronoi(float *xValues, float *yValues, int numPoints, float minX, float maxX, float minY, float maxY, float minDist) +{ + cleanup(); + cleanupEdges(); + int i; + + minDistanceBetweenSites = minDist; + + nsites=numPoints; + plot = 0; + triangulate = 0; + debug = 1; + sorted = 0; + freeinit(&sfl, sizeof (Site)); + + sites = (struct Site *) myalloc(nsites*sizeof( *sites)); + + if(sites == 0) + return false; + + xmin = xValues[0]; + ymin = yValues[0]; + xmax = xValues[0]; + ymax = yValues[0]; + + for(i = 0; i< nsites; i++) + { + sites[i].coord.x = xValues[i]; + sites[i].coord.y = yValues[i]; + sites[i].sitenbr = i; + sites[i].refcnt = 0; + + if(xValues[i] < xmin) + xmin = xValues[i]; + else if(xValues[i] > xmax) + xmax = xValues[i]; + + if(yValues[i] < ymin) + ymin = yValues[i]; + else if(yValues[i] > ymax) + ymax = yValues[i]; + + //printf("\n%f %f\n",xValues[i],yValues[i]); + } + + qsort(sites, nsites, sizeof (*sites), scomp); + + siteidx = 0; + geominit(); + float temp = 0; + if(minX > maxX) + { + temp = minX; + minX = maxX; + maxX = temp; + } + if(minY > maxY) + { + temp = minY; + minY = maxY; + maxY = temp; + } + borderMinX = minX; + borderMinY = minY; + borderMaxX = maxX; + borderMaxY = maxY; + + siteidx = 0; + voronoi(triangulate); + + return true; +} + +bool VoronoiDiagramGenerator::ELinitialize() +{ + int i; + freeinit(&hfl, sizeof **ELhash); + ELhashsize = 2 * sqrt_nsites; + ELhash = (struct Halfedge **) myalloc ( sizeof *ELhash * ELhashsize); + + if(ELhash == 0) + return false; + + for(i=0; i ELleft = (struct Halfedge *)NULL; + ELleftend -> ELright = ELrightend; + ELrightend -> ELleft = ELleftend; + ELrightend -> ELright = (struct Halfedge *)NULL; + ELhash[0] = ELleftend; + ELhash[ELhashsize-1] = ELrightend; + + return true; +} + + +struct Halfedge* VoronoiDiagramGenerator::HEcreate(struct Edge *e,int pm) +{ + struct Halfedge *answer; + answer = (struct Halfedge *) getfree(&hfl); + answer -> ELedge = e; + answer -> ELpm = pm; + answer -> PQnext = (struct Halfedge *) NULL; + answer -> vertex = (struct Site *) NULL; + answer -> ELrefcnt = 0; + return(answer); +} + + +void VoronoiDiagramGenerator::ELinsert(struct Halfedge *lb, struct Halfedge *newHe) +{ + newHe -> ELleft = lb; + newHe -> ELright = lb -> ELright; + (lb -> ELright) -> ELleft = newHe; + lb -> ELright = newHe; +} + +/* Get entry from hash table, pruning any deleted nodes */ +struct Halfedge * VoronoiDiagramGenerator::ELgethash(int b) +{ + struct Halfedge *he; + + if(b<0 || b>=ELhashsize) + return((struct Halfedge *) NULL); + he = ELhash[b]; + if (he == (struct Halfedge *) NULL || he->ELedge != (struct Edge *) DELETED ) + return (he); + + /* Hash table points to deleted half edge. Patch as necessary. */ + ELhash[b] = (struct Halfedge *) NULL; + if ((he -> ELrefcnt -= 1) == 0) + makefree((Freenode*)he, &hfl); + return ((struct Halfedge *) NULL); +} + +struct Halfedge * VoronoiDiagramGenerator::ELleftbnd(struct Point *p) +{ + int i, bucket; + struct Halfedge *he; + + /* Use hash table to get close to desired halfedge */ + bucket = (int)((p->x - xmin)/deltax * ELhashsize); //use the hash function to find the place in the hash map that this HalfEdge should be + + if(bucket<0) bucket =0; //make sure that the bucket position in within the range of the hash array + if(bucket>=ELhashsize) bucket = ELhashsize - 1; + + he = ELgethash(bucket); + if(he == (struct Halfedge *) NULL) //if the HE isn't found, search backwards and forwards in the hash map for the first non-null entry + { + for(i=1; 1 ; i += 1) + { + if ((he=ELgethash(bucket-i)) != (struct Halfedge *) NULL) + break; + if ((he=ELgethash(bucket+i)) != (struct Halfedge *) NULL) + break; + }; + totalsearch += i; + }; + ntry += 1; + /* Now search linear list of halfedges for the correct one */ + if (he==ELleftend || (he != ELrightend && right_of(he,p))) + { + do + { + he = he -> ELright; + } while (he!=ELrightend && right_of(he,p)); //keep going right on the list until either the end is reached, or you find the 1st edge which the point + he = he -> ELleft; //isn't to the right of + } + else //if the point is to the left of the HalfEdge, then search left for the HE just to the left of the point + do + { + he = he -> ELleft; + } while (he!=ELleftend && !right_of(he,p)); + + /* Update hash table and reference counts */ + if(bucket > 0 && bucket ELrefcnt -= 1; + } + ELhash[bucket] = he; + ELhash[bucket] -> ELrefcnt += 1; + }; + return (he); +} + + +/* This delete routine can't reclaim node, since pointers from hash +table may be present. */ +void VoronoiDiagramGenerator::ELdelete(struct Halfedge *he) +{ + (he -> ELleft) -> ELright = he -> ELright; + (he -> ELright) -> ELleft = he -> ELleft; + he -> ELedge = (struct Edge *)DELETED; +} + + +struct Halfedge * VoronoiDiagramGenerator::ELright(struct Halfedge *he) +{ + return (he -> ELright); +} + +struct Halfedge * VoronoiDiagramGenerator::ELleft(struct Halfedge *he) +{ + return (he -> ELleft); +} + + +struct Site * VoronoiDiagramGenerator::leftreg(struct Halfedge *he) +{ + if(he -> ELedge == (struct Edge *)NULL) + return(bottomsite); + return( he -> ELpm == le ? + he -> ELedge -> reg[le] : he -> ELedge -> reg[re]); +} + +struct Site * VoronoiDiagramGenerator::rightreg(struct Halfedge *he) +{ + if(he -> ELedge == (struct Edge *)NULL) //if this halfedge has no edge, return the bottom site (whatever that is) + return(bottomsite); + + //if the ELpm field is zero, return the site 0 that this edge bisects, otherwise return site number 1 + return( he -> ELpm == le ? he -> ELedge -> reg[re] : he -> ELedge -> reg[le]); +} + +void VoronoiDiagramGenerator::geominit() +{ + float sn; + + freeinit(&efl, sizeof(Edge)); + nvertices = 0; + nedges = 0; + sn = (float)nsites+4; + sqrt_nsites = (int)sqrt(sn); + deltay = ymax - ymin; + deltax = xmax - xmin; +} + + +struct Edge * VoronoiDiagramGenerator::bisect(struct Site *s1,struct Site *s2) +{ + float dx,dy,adx,ady; + struct Edge *newedge; + + newedge = (struct Edge *) getfree(&efl); + + newedge -> reg[0] = s1; //store the sites that this edge is bisecting + newedge -> reg[1] = s2; + ref(s1); + ref(s2); + newedge -> ep[0] = (struct Site *) NULL; //to begin with, there are no endpoints on the bisector - it goes to infinity + newedge -> ep[1] = (struct Site *) NULL; + + dx = s2->coord.x - s1->coord.x; //get the difference in x dist between the sites + dy = s2->coord.y - s1->coord.y; + adx = dx>0 ? dx : -dx; //make sure that the difference in positive + ady = dy>0 ? dy : -dy; + newedge -> c = (float)(s1->coord.x * dx + s1->coord.y * dy + (dx*dx + dy*dy)*0.5);//get the slope of the line + + if (adx>ady) + { + newedge -> a = 1.0; newedge -> b = dy/dx; newedge -> c /= dx;//set formula of line, with x fixed to 1 + } + else + { + newedge -> b = 1.0; newedge -> a = dx/dy; newedge -> c /= dy;//set formula of line, with y fixed to 1 + }; + + newedge -> edgenbr = nedges; + + //printf("\nbisect(%d) ((%f,%f) and (%f,%f)",nedges,s1->coord.x,s1->coord.y,s2->coord.x,s2->coord.y); + + nedges += 1; + return(newedge); +} + +//create a new site where the HalfEdges el1 and el2 intersect - note that the Point in the argument list is not used, don't know why it's there +struct Site * VoronoiDiagramGenerator::intersect(struct Halfedge *el1, struct Halfedge *el2, struct Point *p) +{ + struct Edge *e1,*e2, *e; + struct Halfedge *el; + float d, xint, yint; + int right_of_site; + struct Site *v; + + e1 = el1 -> ELedge; + e2 = el2 -> ELedge; + if(e1 == (struct Edge*)NULL || e2 == (struct Edge*)NULL) + return ((struct Site *) NULL); + + //if the two edges bisect the same parent, return null + if (e1->reg[1] == e2->reg[1]) + return ((struct Site *) NULL); + + d = e1->a * e2->b - e1->b * e2->a; + if (-1.0e-10c*e2->b - e2->c*e1->b)/d; + yint = (e2->c*e1->a - e1->c*e2->a)/d; + + if( (e1->reg[1]->coord.y < e2->reg[1]->coord.y) || + (e1->reg[1]->coord.y == e2->reg[1]->coord.y && + e1->reg[1]->coord.x < e2->reg[1]->coord.x) ) + { + el = el1; + e = e1; + } + else + { + el = el2; + e = e2; + }; + + right_of_site = xint >= e -> reg[1] -> coord.x; + if ((right_of_site && el -> ELpm == le) || (!right_of_site && el -> ELpm == re)) + return ((struct Site *) NULL); + + //create a new site at the point of intersection - this is a new vector event waiting to happen + v = (struct Site *) getfree(&sfl); + v -> refcnt = 0; + v -> coord.x = xint; + v -> coord.y = yint; + return(v); +} + +/* returns 1 if p is to right of halfedge e */ +int VoronoiDiagramGenerator::right_of(struct Halfedge *el,struct Point *p) +{ + struct Edge *e; + struct Site *topsite; + int right_of_site, above, fast; + float dxp, dyp, dxs, t1, t2, t3, yl; + + e = el -> ELedge; + topsite = e -> reg[1]; + right_of_site = p -> x > topsite -> coord.x; + if(right_of_site && el -> ELpm == le) return(1); + if(!right_of_site && el -> ELpm == re) return (0); + + if (e->a == 1.0) + { dyp = p->y - topsite->coord.y; + dxp = p->x - topsite->coord.x; + fast = 0; + if ((!right_of_site & (e->b<0.0)) | (right_of_site & (e->b>=0.0)) ) + { above = dyp>= e->b*dxp; + fast = above; + } + else + { above = p->x + p->y*e->b > e-> c; + if(e->b<0.0) above = !above; + if (!above) fast = 1; + }; + if (!fast) + { dxs = topsite->coord.x - (e->reg[0])->coord.x; + above = e->b * (dxp*dxp - dyp*dyp) < + dxs*dyp*(1.0+2.0*dxp/dxs + e->b*e->b); + if(e->b<0.0) above = !above; + }; + } + else /*e->b==1.0 */ + { yl = e->c - e->a*p->x; + t1 = p->y - yl; + t2 = p->x - topsite->coord.x; + t3 = yl - topsite->coord.y; + above = t1*t1 > t2*t2 + t3*t3; + }; + return (el->ELpm==le ? above : !above); +} + + +void VoronoiDiagramGenerator::endpoint(struct Edge *e,int lr,struct Site * s) +{ + e -> ep[lr] = s; + ref(s); + if(e -> ep[re-lr]== (struct Site *) NULL) + return; + + clip_line(e); + + deref(e->reg[le]); + deref(e->reg[re]); + makefree((Freenode*)e, &efl); +} + + +float VoronoiDiagramGenerator::dist(struct Site *s,struct Site *t) +{ + float dx,dy; + dx = s->coord.x - t->coord.x; + dy = s->coord.y - t->coord.y; + return (float)(sqrt(dx*dx + dy*dy)); +} + + +void VoronoiDiagramGenerator::makevertex(struct Site *v) +{ + v -> sitenbr = nvertices; + nvertices += 1; + out_vertex(v); +} + + +void VoronoiDiagramGenerator::deref(struct Site *v) +{ + v -> refcnt -= 1; + if (v -> refcnt == 0 ) + makefree((Freenode*)v, &sfl); +} + +void VoronoiDiagramGenerator::ref(struct Site *v) +{ + v -> refcnt += 1; +} + +//push the HalfEdge into the ordered linked list of vertices +void VoronoiDiagramGenerator::PQinsert(struct Halfedge *he,struct Site * v, float offset) +{ + struct Halfedge *last, *next; + + he -> vertex = v; + ref(v); + he -> ystar = (float)(v -> coord.y + offset); + last = &PQhash[PQbucket(he)]; + while ((next = last -> PQnext) != (struct Halfedge *) NULL && + (he -> ystar > next -> ystar || + (he -> ystar == next -> ystar && v -> coord.x > next->vertex->coord.x))) + { + last = next; + }; + he -> PQnext = last -> PQnext; + last -> PQnext = he; + PQcount += 1; +} + +//remove the HalfEdge from the list of vertices +void VoronoiDiagramGenerator::PQdelete(struct Halfedge *he) +{ + struct Halfedge *last; + + if(he -> vertex != (struct Site *) NULL) + { + last = &PQhash[PQbucket(he)]; + while (last -> PQnext != he) + last = last -> PQnext; + + last -> PQnext = he -> PQnext; + PQcount -= 1; + deref(he -> vertex); + he -> vertex = (struct Site *) NULL; + }; +} + +int VoronoiDiagramGenerator::PQbucket(struct Halfedge *he) +{ + int bucket; + + bucket = (int)((he->ystar - ymin)/deltay * PQhashsize); + if (bucket<0) bucket = 0; + if (bucket>=PQhashsize) bucket = PQhashsize-1 ; + if (bucket < PQmin) PQmin = bucket; + return(bucket); +} + + + +int VoronoiDiagramGenerator::PQempty() +{ + return(PQcount==0); +} + + +struct Point VoronoiDiagramGenerator::PQ_min() +{ + struct Point answer; + + while(PQhash[PQmin].PQnext == (struct Halfedge *)NULL) {PQmin += 1;}; + answer.x = PQhash[PQmin].PQnext -> vertex -> coord.x; + answer.y = PQhash[PQmin].PQnext -> ystar; + return (answer); +} + +struct Halfedge * VoronoiDiagramGenerator::PQextractmin() +{ + struct Halfedge *curr; + + curr = PQhash[PQmin].PQnext; + PQhash[PQmin].PQnext = curr -> PQnext; + PQcount -= 1; + return(curr); +} + + +bool VoronoiDiagramGenerator::PQinitialize() +{ + int i; + + PQcount = 0; + PQmin = 0; + PQhashsize = 4 * sqrt_nsites; + PQhash = (struct Halfedge *) myalloc(PQhashsize * sizeof *PQhash); + + if(PQhash == 0) + return false; + + for(i=0; i head = (struct Freenode *) NULL; + fl -> nodesize = size; +} + +char * VoronoiDiagramGenerator::getfree(struct Freelist *fl) +{ + int i; + struct Freenode *t; + + if(fl->head == (struct Freenode *) NULL) + { + t = (struct Freenode *) myalloc(sqrt_nsites * fl->nodesize); + + if(t == 0) + return 0; + + currentMemoryBlock->next = new FreeNodeArrayList; + currentMemoryBlock = currentMemoryBlock->next; + currentMemoryBlock->memory = t; + currentMemoryBlock->next = 0; + + for(i=0; inodesize), fl); + }; + t = fl -> head; + fl -> head = (fl -> head) -> nextfree; + return((char *)t); +} + + + +void VoronoiDiagramGenerator::makefree(struct Freenode *curr,struct Freelist *fl) +{ + curr -> nextfree = fl -> head; + fl -> head = curr; +} + +void VoronoiDiagramGenerator::cleanup() +{ + if(sites != 0) + { + free(sites); + sites = 0; + } + + FreeNodeArrayList* current=0, *prev = 0; + + current = prev = allMemoryList; + + while(current->next != 0) + { + prev = current; + current = current->next; + free(prev->memory); + delete prev; + prev = 0; + } + + if(current != 0 && current->memory != 0) + { + free(current->memory); + delete current; + } + + allMemoryList = new FreeNodeArrayList; + allMemoryList->next = 0; + allMemoryList->memory = 0; + currentMemoryBlock = allMemoryList; +} + +void VoronoiDiagramGenerator::cleanupEdges() +{ + GraphEdge* geCurrent = 0, *gePrev = 0; + geCurrent = gePrev = allEdges; + + while(geCurrent != 0 && geCurrent->next != 0) + { + gePrev = geCurrent; + geCurrent = geCurrent->next; + delete gePrev; + } + + allEdges = 0; + +} + +void VoronoiDiagramGenerator::pushGraphEdge(float x1, float y1, float x2, float y2) +{ + GraphEdge* newEdge = new GraphEdge; + newEdge->next = allEdges; + allEdges = newEdge; + newEdge->x1 = x1; + newEdge->y1 = y1; + newEdge->x2 = x2; + newEdge->y2 = y2; +} + + +char * VoronoiDiagramGenerator::myalloc(unsigned n) +{ + char *t=0; + t=(char*)malloc(n); + total_alloc += n; + return(t); +} + + +/* for those who don't have Cherry's plot */ +/* #include */ +void VoronoiDiagramGenerator::openpl(){} +void VoronoiDiagramGenerator::line(float x1, float y1, float x2, float y2) +{ + pushGraphEdge(x1,y1,x2,y2); + +} +void VoronoiDiagramGenerator::circle(float x, float y, float radius){} +void VoronoiDiagramGenerator::range(float minX, float minY, float maxX, float maxY){} + + + +void VoronoiDiagramGenerator::out_bisector(struct Edge *e) +{ + + +} + + +void VoronoiDiagramGenerator::out_ep(struct Edge *e) +{ + + +} + +void VoronoiDiagramGenerator::out_vertex(struct Site *v) +{ + +} + + +void VoronoiDiagramGenerator::out_site(struct Site *s) +{ + if(!triangulate & plot & !debug) + circle (s->coord.x, s->coord.y, cradius); + +} + + +void VoronoiDiagramGenerator::out_triple(struct Site *s1, struct Site *s2,struct Site * s3) +{ + +} + + + +void VoronoiDiagramGenerator::plotinit() +{ + float dx,dy,d; + + dy = ymax - ymin; + dx = xmax - xmin; + d = (float)(( dx > dy ? dx : dy) * 1.1); + pxmin = (float)(xmin - (d-dx)/2.0); + pxmax = (float)(xmax + (d-dx)/2.0); + pymin = (float)(ymin - (d-dy)/2.0); + pymax = (float)(ymax + (d-dy)/2.0); + cradius = (float)((pxmax - pxmin)/350.0); + openpl(); + range(pxmin, pymin, pxmax, pymax); +} + + +void VoronoiDiagramGenerator::clip_line(struct Edge *e) +{ + struct Site *s1, *s2; + float x1=0,x2=0,y1=0,y2=0, temp = 0;; + + x1 = e->reg[0]->coord.x; + x2 = e->reg[1]->coord.x; + y1 = e->reg[0]->coord.y; + y2 = e->reg[1]->coord.y; + + //if the distance between the two points this line was created from is less than + //the square root of 2, then ignore it + if(sqrt(((x2 - x1) * (x2 - x1)) + ((y2 - y1) * (y2 - y1))) < minDistanceBetweenSites) + { + return; + } + pxmin = borderMinX; + pxmax = borderMaxX; + pymin = borderMinY; + pymax = borderMaxY; + + if(e -> a == 1.0 && e ->b >= 0.0) + { + s1 = e -> ep[1]; + s2 = e -> ep[0]; + } + else + { + s1 = e -> ep[0]; + s2 = e -> ep[1]; + }; + + if(e -> a == 1.0) + { + y1 = pymin; + if (s1!=(struct Site *)NULL && s1->coord.y > pymin) + { + y1 = s1->coord.y; + } + if(y1>pymax) + { + // printf("\nClipped (1) y1 = %f to %f",y1,pymax); + y1 = pymax; + //return; + } + x1 = e -> c - e -> b * y1; + y2 = pymax; + if (s2!=(struct Site *)NULL && s2->coord.y < pymax) + y2 = s2->coord.y; + + if(y2c) - (e->b) * y2; + if (((x1> pxmax) & (x2>pxmax)) | ((x1 pxmax) + { x1 = pxmax; y1 = (e -> c - x1)/e -> b;}; + if(x1 c - x1)/e -> b;}; + if(x2>pxmax) + { x2 = pxmax; y2 = (e -> c - x2)/e -> b;}; + if(x2 c - x2)/e -> b;}; + } + else + { + x1 = pxmin; + if (s1!=(struct Site *)NULL && s1->coord.x > pxmin) + x1 = s1->coord.x; + if(x1>pxmax) + { + //printf("\nClipped (3) x1 = %f to %f",x1,pxmin); + //return; + x1 = pxmax; + } + y1 = e -> c - e -> a * x1; + x2 = pxmax; + if (s2!=(struct Site *)NULL && s2->coord.x < pxmax) + x2 = s2->coord.x; + if(x2 c - e -> a * x2; + if (((y1> pymax) & (y2>pymax)) | ((y1 pymax) + { y1 = pymax; x1 = (e -> c - y1)/e -> a;}; + if(y1 c - y1)/e -> a;}; + if(y2>pymax) + { y2 = pymax; x2 = (e -> c - y2)/e -> a;}; + if(y2 c - y2)/e -> a;}; + }; + + //printf("\nPushing line (%f,%f,%f,%f)",x1,y1,x2,y2); + line(x1,y1,x2,y2); +} + + +/* implicit parameters: nsites, sqrt_nsites, xmin, xmax, ymin, ymax, +deltax, deltay (can all be estimates). +Performance suffers if they are wrong; better to make nsites, +deltax, and deltay too big than too small. (?) */ + +bool VoronoiDiagramGenerator::voronoi(int triangulate) +{ + struct Site *newsite, *bot, *top, *temp, *p; + struct Site *v; + struct Point newintstar; + int pm; + struct Halfedge *lbnd, *rbnd, *llbnd, *rrbnd, *bisector; + struct Edge *e; + + PQinitialize(); + bottomsite = nextone(); + out_site(bottomsite); + bool retval = ELinitialize(); + + if(!retval) + return false; + + newsite = nextone(); + while(1) + { + + if(!PQempty()) + newintstar = PQ_min(); + + //if the lowest site has a smaller y value than the lowest vector intersection, process the site + //otherwise process the vector intersection + + if (newsite != (struct Site *)NULL && (PQempty() || newsite -> coord.y < newintstar.y + || (newsite->coord.y == newintstar.y && newsite->coord.x < newintstar.x))) + {/* new site is smallest - this is a site event*/ + out_site(newsite); //output the site + lbnd = ELleftbnd(&(newsite->coord)); //get the first HalfEdge to the LEFT of the new site + rbnd = ELright(lbnd); //get the first HalfEdge to the RIGHT of the new site + bot = rightreg(lbnd); //if this halfedge has no edge, , bot = bottom site (whatever that is) + e = bisect(bot, newsite); //create a new edge that bisects + bisector = HEcreate(e, le); //create a new HalfEdge, setting its ELpm field to 0 + ELinsert(lbnd, bisector); //insert this new bisector edge between the left and right vectors in a linked list + + if ((p = intersect(lbnd, bisector)) != (struct Site *) NULL) //if the new bisector intersects with the left edge, remove the left edge's vertex, and put in the new one + { + PQdelete(lbnd); + PQinsert(lbnd, p, dist(p,newsite)); + }; + lbnd = bisector; + bisector = HEcreate(e, re); //create a new HalfEdge, setting its ELpm field to 1 + ELinsert(lbnd, bisector); //insert the new HE to the right of the original bisector earlier in the IF stmt + + if ((p = intersect(bisector, rbnd)) != (struct Site *) NULL) //if this new bisector intersects with the + { + PQinsert(bisector, p, dist(p,newsite)); //push the HE into the ordered linked list of vertices + }; + newsite = nextone(); + } + else if (!PQempty()) /* intersection is smallest - this is a vector event */ + { + lbnd = PQextractmin(); //pop the HalfEdge with the lowest vector off the ordered list of vectors + llbnd = ELleft(lbnd); //get the HalfEdge to the left of the above HE + rbnd = ELright(lbnd); //get the HalfEdge to the right of the above HE + rrbnd = ELright(rbnd); //get the HalfEdge to the right of the HE to the right of the lowest HE + bot = leftreg(lbnd); //get the Site to the left of the left HE which it bisects + top = rightreg(rbnd); //get the Site to the right of the right HE which it bisects + + out_triple(bot, top, rightreg(lbnd)); //output the triple of sites, stating that a circle goes through them + + v = lbnd->vertex; //get the vertex that caused this event + makevertex(v); //set the vertex number - couldn't do this earlier since we didn't know when it would be processed + endpoint(lbnd->ELedge,lbnd->ELpm,v); //set the endpoint of the left HalfEdge to be this vector + endpoint(rbnd->ELedge,rbnd->ELpm,v); //set the endpoint of the right HalfEdge to be this vector + ELdelete(lbnd); //mark the lowest HE for deletion - can't delete yet because there might be pointers to it in Hash Map + PQdelete(rbnd); //remove all vertex events to do with the right HE + ELdelete(rbnd); //mark the right HE for deletion - can't delete yet because there might be pointers to it in Hash Map + pm = le; //set the pm variable to zero + + if (bot->coord.y > top->coord.y) //if the site to the left of the event is higher than the Site + { //to the right of it, then swap them and set the 'pm' variable to 1 + temp = bot; + bot = top; + top = temp; + pm = re; + } + e = bisect(bot, top); //create an Edge (or line) that is between the two Sites. This creates + //the formula of the line, and assigns a line number to it + bisector = HEcreate(e, pm); //create a HE from the Edge 'e', and make it point to that edge with its ELedge field + ELinsert(llbnd, bisector); //insert the new bisector to the right of the left HE + endpoint(e, re-pm, v); //set one endpoint to the new edge to be the vector point 'v'. + //If the site to the left of this bisector is higher than the right + //Site, then this endpoint is put in position 0; otherwise in pos 1 + deref(v); //delete the vector 'v' + + //if left HE and the new bisector don't intersect, then delete the left HE, and reinsert it + if((p = intersect(llbnd, bisector)) != (struct Site *) NULL) + { + PQdelete(llbnd); + PQinsert(llbnd, p, dist(p,bot)); + }; + + //if right HE and the new bisector don't intersect, then reinsert it + if ((p = intersect(bisector, rrbnd)) != (struct Site *) NULL) + { + PQinsert(bisector, p, dist(p,bot)); + }; + } + else break; + }; + + + + + for(lbnd=ELright(ELleftend); lbnd != ELrightend; lbnd=ELright(lbnd)) + { + e = lbnd -> ELedge; + + clip_line(e); + }; + + cleanup(); + + return true; + +} + + +int scomp(const void *p1,const void *p2) +{ + struct Point *s1 = (Point*)p1, *s2=(Point*)p2; + if(s1 -> y < s2 -> y) return(-1); + if(s1 -> y > s2 -> y) return(1); + if(s1 -> x < s2 -> x) return(-1); + if(s1 -> x > s2 -> x) return(1); + return(0); +} + +/* return a single in-storage site */ +struct Site * VoronoiDiagramGenerator::nextone() +{ + struct Site *s; + if(siteidx < nsites) + { + s = &sites[siteidx]; + siteidx += 1; + return(s); + } + else + return( (struct Site *)NULL); +} + diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 4ce60b5..daedd8f 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -240,6 +240,9 @@ static int buzz_register_hooks() buzzvm_pushs(VM, buzzvm_string_register(VM, "reset_rc", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_resetrc)); buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "voronoi", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::voronoi_center)); + buzzvm_gstore(VM); return VM->state; } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 8cdfa92..ad91e83 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -8,6 +8,7 @@ #include "buzzuav_closures.h" #include "math.h" +#include "VoronoiDiagramGenerator.h" namespace buzzuav_closures { @@ -132,6 +133,18 @@ void rb_from_gps(double nei[], double out[], double cur[]) out[2] = 0.0; } +void gps_from_vec(double vec[], double gps[]) { + double Vrange = sqrt(vec[0] * vec[0] + vec[1] * vec[1]); + double Vbearing = constrainAngle(atan2(vec[1], vec[0])); + double latR = cur_pos[0]*M_PI/180.0; + double lonR = cur_pos[1]*M_PI/180.0; + double target_lat = asin(sin(latR) * cos(Vrange/EARTH_RADIUS) + cos(latR) * sin(Vrange/EARTH_RADIUS) * cos(Vbearing)); + double target_lon = lonR + atan2(sin(Vbearing) * sin(Vrange/EARTH_RADIUS) * cos(latR), cos(Vrange/EARTH_RADIUS) - sin(latR) * sin(target_lat)); + gps[0] = target_lat*180.0/M_PI; + gps[1] = target_lon*180.0/M_PI; + gps[2] = cur_pos[2]; +} + void parse_gpslist() /* / parse a csv of GPS targets @@ -235,6 +248,134 @@ int buzz_exportmap(buzzvm_t vm) return buzzvm_ret0(vm); } +int voronoi_center(buzzvm_t vm) { + float *xValues;//[4] = {-22, -17, 4,22}; + float *yValues;//[4] = {-9, 31,13,-5}; + float minx, miny, maxx, maxy; + buzzvm_lnum_assert(vm, 1); + // Get the parameter + buzzvm_lload(vm, 1); + buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary + buzzobj_t t = buzzvm_stack_at(vm, 1); + + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "maxx", 1)); + buzzvm_tget(vm); + maxx = buzzvm_stack_at(vm, 1)->f.value; + buzzvm_pop(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "maxy", 1)); + buzzvm_tget(vm); + maxy = buzzvm_stack_at(vm, 1)->f.value; + buzzvm_pop(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "minx", 1)); + buzzvm_tget(vm); + minx = buzzvm_stack_at(vm, 1)->f.value; + buzzvm_pop(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "miny", 1)); + buzzvm_tget(vm); + miny = buzzvm_stack_at(vm, 1)->f.value; + buzzvm_pop(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "oa", 1)); + buzzvm_tget(vm); + float offset_angle = buzzvm_stack_at(vm, 1)->f.value; + buzzvm_pop(vm); + + long count = buzzdict_size(t->t.value)-5; + xValues = new float[count]; + yValues = new float[count]; + for(int32_t i = 0; i < count; ++i) { + buzzvm_dup(vm); + buzzvm_pushi(vm, i); + buzzvm_tget(vm); + + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1)); + buzzvm_tget(vm); + //ROS_INFO("---x-->%f",buzzvm_stack_at(vm, 1)->f.value); + xValues[i] = buzzvm_stack_at(vm, 1)->f.value; + buzzvm_pop(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1)); + buzzvm_tget(vm); + //ROS_INFO("---y-->%f",buzzvm_stack_at(vm, 1)->f.value); + yValues[i] = buzzvm_stack_at(vm, 1)->f.value; + buzzvm_pop(vm); + + buzzvm_pop(vm); + } + + VoronoiDiagramGenerator vdg; + vdg.generateVoronoi(xValues,yValues,count, minx, maxx, miny, maxy,3); + + vdg.resetIterator(); + + float x1,y1,x2,y2; + map> edges; + while(vdg.getNext(x1,y1,x2,y2)) + { + ROS_INFO("GOT Line (%f,%f)->(%f,%f)\n",x1,y1,x2, y2); + edges.insert(make_pair(sqrt((x2+x1)*(x2+x1)/4+(y2+y1)*(y2+y1)/4), std::array{x1,y1,x2,y2})); + } + double center_dist[2] = {0,0}; + float closest_points[8]; + int nit = 1; + float prev; + map>::iterator it; + int got3 = 0; + for (it = edges.begin(); it != edges.end(); ++it) + { + if(nit == 1) { + //center_dist[0] += it->second[0] + it->second[2]; + //center_dist[1] += it->second[1] + it->second[3]; + closest_points[0] = it->second[0]; closest_points[1] = it->second[1]; closest_points[2] = it->second[2]; closest_points[3] = it->second[3]; + ROS_INFO("USE Line (%f,%f)->(%f,%f): %f\n",it->second[0],it->second[1],it->second[2], it->second[3], it->first); + prev = it->first; + } else if(nit == 2) { + map>::iterator it_prev = edges.find(prev); + if(it->second[0]!=it_prev->second[0] && it->second[1]!=it_prev->second[1] && it->second[0]!=it_prev->second[2] && it->second[1]!=it_prev->second[3]){ + //center_dist[0] += it->second[0]; + //center_dist[1] += it->second[1]; + closest_points[4] = it->second[0]; closest_points[5] = it->second[1]; + ROS_INFO("USE Point (%f,%f): %f\n",it->second[0],it->second[1], it->first); + got3 = 1; + } + if(it->second[2]!=it_prev->second[0] && it->second[3]!=it_prev->second[1] && it->second[2]!=it_prev->second[2] && it->second[3]!=it_prev->second[3]){ + //center_dist[0] += it->second[2]; + //center_dist[1] += it->second[3]; + if(!got3) { + closest_points[4] = it->second[2]; closest_points[5] = it->second[3]; + } else { + closest_points[6] = it->second[2]; closest_points[7] = it->second[3]; + got3=2; + } + ROS_INFO("USE Point (%f,%f): %f (%i)\n",it->second[2],it->second[3], it->first, got3); + } + } else + break; + nit++; + } + if(got3==2) + center_dist[0]=(closest_points[0]+closest_points[2]+closest_points[4]+closest_points[6])/4; + else + center_dist[0]=(closest_points[0]+closest_points[2]+closest_points[4])/3; + if(got3==2) + center_dist[1]=(closest_points[1]+closest_points[3]+closest_points[5]+closest_points[7])/4; + else + center_dist[1]=(closest_points[1]+closest_points[3]+closest_points[5])/3; + center_dist[0]=center_dist[0]*cos(offset_angle)-center_dist[1]*sin(offset_angle); + center_dist[1]=center_dist[0]*sin(offset_angle)+center_dist[1]*cos(offset_angle); + double gps[3]; + gps_from_vec(center_dist, gps); + ROS_INFO("[%i] Voronoi cell center: %f, %f, %f, %f", buzz_utility::get_robotid(), center_dist[0], center_dist[1], gps[0], gps[1]); + set_gpsgoal(gps); + + return buzzvm_ret0(vm); +} + int buzzuav_moveto(buzzvm_t vm) /* / Buzz closure to move following a 3D vector + Yaw