Commit 73f20526 authored by Ulrich Kemloh's avatar Ulrich Kemloh

Added voronoi diagram generator

parent 181d9599
......@@ -70,12 +70,14 @@ xsi:noNamespaceSchemaLocation="">
<!--persons information and distribution -->
<agents operational_model_id="2">
<group group_id="0" room_id="0" subroom_id="0" number="50" router_id="1" agent_parameter_id="1"
risk_tolerance_mean="0.8" risk_tolerance_sigma="0.01"/>
<group group_id="0" room_id="0" subroom_id="0" number="5" router_id="1" agent_parameter_id="1"
risk_tolerance_mean="0.8" risk_tolerance_sigma="0.01" x_min="14" x_max="16" y_min="14" y_max="16"/>
<group group_id="1" room_id="0" subroom_id="0" number="50" router_id="1" agent_parameter_id="1" />
<agents_sources><!-- frequency in persons/minute -->
<source id="1" frequency="1" agents_max="100" group_id="0" caption="source 1">
<source id="1" frequency="5" agents_max="100" group_id="0" caption="source 1">
......@@ -103,6 +103,11 @@ void AgentsSource::SetStartDistribution(std::shared_ptr<StartDistribution> start
const std::shared_ptr<StartDistribution> AgentsSource::GetStartDistribution() const
return _startDistribution;
void AgentsSource::Dump() const
......@@ -68,6 +68,8 @@ public:
int GetId() const;
int GetMaxAgents() const;
void SetStartDistribution(std::shared_ptr<StartDistribution>);
const std::shared_ptr<StartDistribution> GetStartDistribution() const;
......@@ -8,6 +8,8 @@
#include "AgentsSourcesManager.h"
#include "Pedestrian.h"
#include "AgentsQueue.h"
#include "StartDistribution.h"
#include "PedDistributor.h"
#include "AgentsSource.h"
#include "../voronoi/VoronoiDiagramGenerator.h"
#include "../geometry/Building.h"
......@@ -31,79 +33,146 @@ void AgentsSourcesManager::operator()(int value)
//the loop is updated each second.
//it might be better to use a timer
bool finished=false;
long updateFrequency=5;// 1 second
bool finished = false;
long updateFrequency = 5; // 1 second
int current_time = Pedestrian::GetGlobalTime();
if ( (current_time != _lastUpdateTime) &&
((current_time % updateFrequency) == 0))
if ((current_time != _lastUpdateTime) && ((current_time % updateFrequency) == 0))
for (const auto& src: _sources)
finished = true;
for (const auto& src : _sources)
if (src->GetPoolSize())
vector<Pedestrian*> peds;
//ComputeBestPositionRandom(src.get(), peds);
// compute the optimal position for insertion
for (auto&& ped: peds)
for (auto&& ped : peds)
//ComputeBestPosition(src.get(), ped);
finished = false;
//cout<<"Agents generated: "<<peds.size()<<endl;
_lastUpdateTime = current_time;
//wait some time
}while (!finished);
} while (!finished);
void AgentsSourcesManager::ComputeBestPosition(AgentsSource* src)
void AgentsSourcesManager::ComputeBestPositionVoronoi(AgentsSource* src, Pedestrian* agent)
int roomID=0;
int subroomID=0;
auto dist = src->GetStartDistribution();
double bounds[4];
int roomID = dist->GetRoomId();
int subroomID = dist->GetSubroomID();
//Get all pedestrians in that location
vector<Pedestrian*> peds;
_building->GetPedestrians(roomID, subroomID, peds);
//filter the points that are not in the boundaries
for (auto&& iter = peds.begin(); iter != peds.end();)
const Point& pos = (*iter)->GetPos();
if ((bounds[0] <= pos._x && pos._x <= bounds[1])
&& (bounds[1] <= pos._y && pos._y <= bounds[2]))
iter = peds.erase(iter);
cout << "removing..." << endl;
//special case with 1, 2 or only three pedestrians in the area
if (peds.size() < 3)
//random position in the area
// compute the cells and cut with the bounds
const int count=peds.size();
const int count = peds.size();
float xValues[count];
float yValues[count];
for(int i=0;i<count;i++)
for (int i = 0; i < count; i++)
xValues[i] = peds[i]->GetPos()._x;
yValues[i] = peds[i]->GetPos()._y;
VoronoiDiagramGenerator vdg;
vdg.generateVoronoi(xValues,yValues,count, -100,100,-100,100,3);
vdg.generateVoronoi(xValues, yValues, count, bounds[0], bounds[1], bounds[2], bounds[3], 3);
float x1,y1;
//collect the positions
vector<Point> positions;
float x1, y1;
while (vdg.getNextVertex(x1, y1))
printf("GOT Point (%f,%f)\n", x1, y1);
positions.push_back(Point(x1, y1));
//look for the biggest spot
map<double, Point> map_dist_to_position;
for (auto&& pos : positions)
double min_dist = FLT_MAX;
for (auto&& ped : peds)
double dist = (pos - ped->GetPos()).NormSquare();
if (dist < min_dist)
min_dist = dist;
map_dist_to_position[min_dist] = pos;
//list the result
for (auto&& mp : map_dist_to_position)
printf("GOT Point (%f,%f)\n",x1,y1);
cout << "dist: " << mp.first << " pos: " << mp.second.toString() << endl;
//agent->SetPos(mp.second, true);
//the elements are ordered.
// so the last one has the largest distance
if (!map_dist_to_position.empty())
agent->SetPos(map_dist_to_position.rbegin()->second, true);
cout << "position:" << agent->GetPos().toString() << endl;
cout << "position not set:" << endl;
cout << "size: " << map_dist_to_position.size() << endl;
// float x1,y1,x2,y2;
......@@ -112,9 +181,77 @@ void AgentsSourcesManager::ComputeBestPosition(AgentsSource* src)
//compute the best position
void AgentsSourcesManager::ComputeBestPositionRandom(AgentsSource* src, std::vector<Pedestrian*>& peds)
//generate the agents with default positions
auto dist=src->GetStartDistribution();
auto subroom=_building->GetRoom(dist->GetRoomId())->GetSubRoom(dist->GetSubroomID());
vector<Point> positions=PedDistributor::PossiblePositions(*subroom);
double bounds[4];
//int roomID = dist->GetRoomId();
//int subroomID = dist->GetSubroomID();
// first default Position
for(const auto& ped: peds)
ped->Dump(ped->GetID()); continue;
int index = -1;
//in the case a range was specified
for (unsigned int a=0;a<positions.size();a++)
Point pos=positions[a];
if((bounds[0]<=pos._x) &&
(pos._x <= bounds[1])&&
(bounds[2]<=pos._y) &&
(pos._y < bounds[3]))
Log->Write("ERROR:\t Cannot distribute pedestrians in the mentioned area [%0.2f,%0.2f,%0.2f,%0.2f]",
Log->Write("ERROR:\t Specifying a subroom_id might help");
const Point& pos = positions[index];
ped->SetPos(pos,true); //true for the initial position
positions.erase(positions.begin() + index);
// const Point& start_pos = Point(_startX, _startY);
// if ((std::isnan(start_pos._x) == 0) && (std::isnan(start_pos._y) == 0))
// {
// if (_building->GetRoom(ped->GetRoomID())->GetSubRoom(ped->GetSubRoomID())->IsInSubRoom(
// start_pos) == false)
// {
// Log->Write(
// "ERROR: \t cannot distribute pedestrian %d in Room %d at fixed position %s",
// *pid, GetRoomId(), start_pos.toString().c_str());
// Log->Write(
// "ERROR: \t Make sure that the position is inside the geometry and belongs to the specified room / subroom");
// exit(EXIT_FAILURE);
// }
// ped->SetPos(start_pos, true); //true for the initial position
// Log->Write("INFO: \t fixed position for ped %d in Room %d %s", *pid, GetRoomId(),
// start_pos.toString().c_str());
// }
void AgentsSourcesManager::AddSource(std::shared_ptr<AgentsSource> src)
......@@ -127,5 +264,5 @@ const std::vector<std::shared_ptr<AgentsSource> >& AgentsSourcesManager::GetSour
void AgentsSourcesManager::SetBuilding(Building* building)
_building = building;
......@@ -14,6 +14,7 @@
//Forward declarations
class AgentsSource;
class Building;
class Pedestrian;
class AgentsSourcesManager
......@@ -61,7 +62,8 @@ private:
Building* _building=nullptr;
void ComputeBestPosition(AgentsSource* src);
void ComputeBestPositionVoronoi(AgentsSource* src, Pedestrian* agent);
void ComputeBestPositionRandom(AgentsSource* src, std::vector<Pedestrian*>& peds);
This diff is collapsed.
MapManager library for the conversion, manipulation and analysis
of maps used in Mobile Robotics research.
Copyright (C) 2005 Shane O'Sullivan
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#include <math.h>
#include <stdlib.h>
#include <string.h>
//#include "../logger/Logger.h"
#ifndef NULL
#define NULL 0
#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 PointVDG
float x,y;
struct Point3
float x,y,z;
int count;
struct VertexLink
PointVDG coord;
PointVDG v[3];
int count;
// structure used both for sites and for vertices
struct Site
struct PointVDG coord;
int sitenbr;
int refcnt;
int overallRefcnt;
struct Edge
float a,b,c;
struct Site *ep[2];
struct Site *reg[2];
int edgenbr;
struct GraphEdge
float x1,y1,x2,y2;
// long v1,v2; //vertices that this was created from
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
bool generateVoronoi(float *xValues, float *yValues, int numPoints,
float minX, float maxX, float minY, float maxY, float minDist,bool genVectorInfo=true);
//By default, the delaunay triangulation is NOT generated
void setGenerateDelaunay(bool genDel);
//By default, the voronoi diagram IS generated
void setGenerateVoronoi(bool genVor);
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;
// LOG<<"getNext returned the edge ("<<x1<<","<<y1<<") -> ("<<x2<<","<<y2<<")";
iteratorEdges = iteratorEdges->next;
return true;
void resetDelaunayEdgesIterator()
iteratorDelaunayEdges = delaunayEdges;
//LOG<<"resetDelaunayEdgesIterator set iteratorDelaunayEdges = "<<iteratorDelaunayEdges;
bool getNextDelaunay(float& x1, float& y1, float& x2, float& y2)
if(iteratorDelaunayEdges == 0)
//LOG<<"iteratorDelaunayEdges = 0, returning false";
return false;
x1 = iteratorDelaunayEdges->x1;
x2 = iteratorDelaunayEdges->x2;
y1 = iteratorDelaunayEdges->y1;
y2 = iteratorDelaunayEdges->y2;
iteratorDelaunayEdges = iteratorDelaunayEdges->next;
//LOG<<"getNextDelaunay returned the edge ("<<x1<<","<<y1<<") -> ("<<x2<<","<<y2<<")";
return true;
void resetVertexPairIterator()
currentVertexLink = 0;
bool getNextVertexPair(float& x1, float& y1, float& x2, float& y2);
void resetVerticesIterator()
currentVertex = 0;
bool getNextVertex(float& x, float& y)
if(finalVertices == 0)
return false;
if(currentVertex >= sizeOfFinalVertices) return false;
x = finalVertices[currentVertex].x;
y = finalVertices[currentVertex].y;
return true;
void reset();
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 PointVDG 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(bool genVectorInfo);
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 PointVDG *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 * VoronoiDiagramGenerator::ELgethash(int b);
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 PointVDG *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 PointVDG *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 pushDelaunayGraphEdge(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);
void insertVertexAddress(long vertexNum, struct Site* address);
void insertVertexLink(long vertexNum, long vertexLinkedTo);
void generateVertexLinks();
bool genDelaunay;
bool genVoronoi;
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;