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

Added voronoi diagram generator

parent 181d9599
......@@ -70,12 +70,14 @@ xsi:noNamespaceSchemaLocation="http://xsd.jupedsim.org/0.6/jps_ini_core.xsd">
<!--persons information and distribution -->
<agents operational_model_id="2">
<agents_distribution>
<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_distribution>
<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">
</source>
</agents_sources>
......
......@@ -103,6 +103,11 @@ void AgentsSource::SetStartDistribution(std::shared_ptr<StartDistribution> start
_startDistribution=startDistribution;
}
const std::shared_ptr<StartDistribution> AgentsSource::GetStartDistribution() const
{
return _startDistribution;
}
void AgentsSource::Dump() const
{
Log->Write("\n--------------------------");
......
......@@ -68,6 +68,8 @@ public:
int GetId() const;
int GetMaxAgents() const;
void SetStartDistribution(std::shared_ptr<StartDistribution>);
const std::shared_ptr<StartDistribution> GetStartDistribution() const;
private:
......
......@@ -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
do
{
int current_time = Pedestrian::GetGlobalTime();
if ( (current_time != _lastUpdateTime) &&
((current_time % updateFrequency) == 0))
if ((current_time != _lastUpdateTime) && ((current_time % updateFrequency) == 0))
{
finished=true;
for (const auto& src: _sources)
finished = true;
for (const auto& src : _sources)
{
if(src->GetPoolSize())
if (src->GetPoolSize())
{
vector<Pedestrian*> peds;
src->GenerateByFrequency(peds);
AgentsQueue::Add(peds);
ComputeBestPosition(src.get());
//ComputeBestPositionRandom(src.get(), peds);
// compute the optimal position for insertion
for (auto&& ped: peds)
for (auto&& ped : peds)
{
//ComputeBestPosition(src.get(), ped);
ped->SetPos(Point(15,15),true);
//ped->Dump(ped->GetID());
}
finished=false;
finished = false;
//cout<<"Agents generated: "<<peds.size()<<endl;
}
//src->Dump();
//src->Dump();exit(0);
}
_lastUpdateTime = current_time;
}
//wait some time
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}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];
dist->Getbounds(bounds);
int roomID = dist->GetRoomId();
int subroomID = dist->GetSubroomID();
//Get all pedestrians in that location
vector<Pedestrian*> peds;
_building->GetPedestrians(roomID,subroomID,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;
exit(0);
}
else
{
++iter;
}
}
//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;
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);
vdg.resetIterator();
vdg.resetVerticesIterator();
float x1,y1;
printf("\n------vertices---------\n");
while(vdg.getNextVertex(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;
//exit(0);
}
else
{
cout << "position not set:" << endl;
cout << "size: " << map_dist_to_position.size() << endl;
exit(0);
}
//exit(0);
// float x1,y1,x2,y2;
//while(vdg.getNext(x1,y1,x2,y2))
......@@ -112,9 +181,77 @@ void AgentsSourcesManager::ComputeBestPosition(AgentsSource* src)
//
//}
//compute the best position
exit(0);
//exit(0);
}
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];
dist->Getbounds(bounds);
//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]))
{
index=a;
break;
}
}
if(index==-1)
{
if(positions.size())
{
Log->Write("ERROR:\t Cannot distribute pedestrians in the mentioned area [%0.2f,%0.2f,%0.2f,%0.2f]",
bounds[0],bounds[1],bounds[2],bounds[3]);
Log->Write("ERROR:\t Specifying a subroom_id might help");
}
}
else
{
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)
{
_sources.push_back(src);
......@@ -127,5 +264,5 @@ const std::vector<std::shared_ptr<AgentsSource> >& AgentsSourcesManager::GetSour
void AgentsSourcesManager::SetBuilding(Building* building)
{
_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;
private:
void ComputeBestPosition(AgentsSource* src);
void ComputeBestPositionVoronoi(AgentsSource* src, Pedestrian* agent);
void ComputeBestPositionRandom(AgentsSource* src, std::vector<Pedestrian*>& peds);
};
#endif /* AGENTSSOURCESMANAGER_H_ */
/*
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
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
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
email: shaneosullivan1@gmail.com
*/
#include "VoronoiDiagramGenerator.h"
#include <iostream>
using namespace std;
#define LOG cout
VoronoiDiagramGenerator::VoronoiDiagramGenerator()
{
//GET_FILE_LOG
//LOGGING_OFF
siteidx = 0;
sites = 0;
allMemoryList = new FreeNodeArrayList;
allMemoryList->memory = 0;
allMemoryList->next = 0;
currentMemoryBlock = allMemoryList;
allEdges = 0;
iteratorEdges = 0;
sites = 0;
ELhash = 0;
PQhash = 0;
minDistanceBetweenSites = 0;
vertexLinks = 0;
vertices = 0;
finalVertexLinks =0;
finalVertices = 0;
setGenerateVoronoi(true);
setGenerateDelaunay(false);
delaunayEdges = 0;
iteratorDelaunayEdges = 0;
}
VoronoiDiagramGenerator::~VoronoiDiagramGenerator()
{
LOG<<"In ~VoronoiDiagramGenerator()"<<endl;
reset();
/*
cleanup();
cleanupEdges();
if(allMemoryList != 0)
delete allMemoryList;
if(finalVertices != 0)
free(finalVertices);
if(vertexLinks != 0)
free(vertexLinks);
if(vertices != 0)
free(vertices);
if(finalVertexLinks != 0)
free(finalVertexLinks);
*/
}
void VoronoiDiagramGenerator::reset()
{
LOG<<"In VoronoiDiagramGenerator::reset()"<<endl;
cleanup();
cleanupEdges();
if(allMemoryList != 0)
delete allMemoryList;
if(finalVertices != 0)
free(finalVertices);
if(vertexLinks != 0)
free(vertexLinks);
if(vertices != 0)
free(vertices);
if(finalVertexLinks != 0)
free(finalVertexLinks);
allMemoryList = 0;
finalVertices = 0;
vertexLinks = 0;
vertices = 0;
finalVertexLinks = 0;
LOG<<"At end of VoronoiDiagramGenerator::reset()"<<endl;
}
void VoronoiDiagramGenerator::setGenerateDelaunay(bool genDel)
{
genDelaunay = genDel;
}
void VoronoiDiagramGenerator::setGenerateVoronoi(bool genVor)
{
genVoronoi= genVor;
}
bool VoronoiDiagramGenerator::generateVoronoi(float *xValues, float *yValues, int numPoints, float minX,
float maxX, float minY, float maxY, float minDist, bool genVertexInfo)
{
cleanup();
cleanupEdges();
int i;
minDistanceBetweenSites = minDist;
nsites=numPoints;
plot = 0;
triangulate = 0;
debug = 1;
sorted = 0;
if(sites != 0)
free(sites);
freeinit(&sfl, sizeof (Site));
sites = (struct Site *) myalloc(nsites*sizeof( *sites));
if(finalVertices != 0)
free(finalVertices);
if(vertexLinks != 0)
free(vertexLinks);
if(vertices != 0)
free(vertices);
if(finalVertexLinks != 0)
free(finalVertexLinks);
vertexLinks = 0;
vertices = 0;
finalVertexLinks =0;
finalVertices = 0;
sizeOfVertices = 0;
sizeOfVertexLinks = 0;
sizeOfFinalVertices = 0;
sizeOfFinalVertexLinks = 0;
if(sites == 0)
{
LOG<<"generateVoronoi returning false 1";
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); //undo
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;
LOG<<"About to call voronoi("<<triangulate<<")";
voronoi(genVertexInfo); //uncomment
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<ELhashsize; i +=1) ELhash[i] = (struct Halfedge *)NULL;
ELleftend = HEcreate( (struct Edge *)NULL, 0);
ELrightend = HEcreate( (struct Edge *)NULL, 0);
ELleftend -> 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 PointVDG *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 <ELhashsize-1)
{
if(ELhash[bucket] != (struct Halfedge *) NULL)
{
ELhash[bucket] -> ELrefcnt -= 1;
}
ELhash[bucket] = he;
ELhash[bucket] -> ELrefcnt += 1;
};