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"> ...@@ -70,12 +70,14 @@ xsi:noNamespaceSchemaLocation="http://xsd.jupedsim.org/0.6/jps_ini_core.xsd">
<!--persons information and distribution --> <!--persons information and distribution -->
<agents operational_model_id="2"> <agents operational_model_id="2">
<agents_distribution> <agents_distribution>
<group group_id="0" room_id="0" subroom_id="0" number="50" router_id="1" agent_parameter_id="1" <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"/> 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_distribution>
<agents_sources><!-- frequency in persons/minute --> <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> </source>
</agents_sources> </agents_sources>
......
...@@ -103,6 +103,11 @@ void AgentsSource::SetStartDistribution(std::shared_ptr<StartDistribution> start ...@@ -103,6 +103,11 @@ void AgentsSource::SetStartDistribution(std::shared_ptr<StartDistribution> start
_startDistribution=startDistribution; _startDistribution=startDistribution;
} }
const std::shared_ptr<StartDistribution> AgentsSource::GetStartDistribution() const
{
return _startDistribution;
}
void AgentsSource::Dump() const void AgentsSource::Dump() const
{ {
Log->Write("\n--------------------------"); Log->Write("\n--------------------------");
......
...@@ -68,6 +68,8 @@ public: ...@@ -68,6 +68,8 @@ public:
int GetId() const; int GetId() const;
int GetMaxAgents() const; int GetMaxAgents() const;
void SetStartDistribution(std::shared_ptr<StartDistribution>); void SetStartDistribution(std::shared_ptr<StartDistribution>);
const std::shared_ptr<StartDistribution> GetStartDistribution() const;
private: private:
......
...@@ -8,6 +8,8 @@ ...@@ -8,6 +8,8 @@
#include "AgentsSourcesManager.h" #include "AgentsSourcesManager.h"
#include "Pedestrian.h" #include "Pedestrian.h"
#include "AgentsQueue.h" #include "AgentsQueue.h"
#include "StartDistribution.h"
#include "PedDistributor.h"
#include "AgentsSource.h" #include "AgentsSource.h"
#include "../voronoi/VoronoiDiagramGenerator.h" #include "../voronoi/VoronoiDiagramGenerator.h"
#include "../geometry/Building.h" #include "../geometry/Building.h"
...@@ -31,79 +33,146 @@ void AgentsSourcesManager::operator()(int value) ...@@ -31,79 +33,146 @@ void AgentsSourcesManager::operator()(int value)
{ {
//the loop is updated each second. //the loop is updated each second.
//it might be better to use a timer //it might be better to use a timer
bool finished=false; bool finished = false;
long updateFrequency=5;// 1 second long updateFrequency = 5; // 1 second
do do
{ {
int current_time = Pedestrian::GetGlobalTime(); int current_time = Pedestrian::GetGlobalTime();
if ( (current_time != _lastUpdateTime) && if ((current_time != _lastUpdateTime) && ((current_time % updateFrequency) == 0))
((current_time % updateFrequency) == 0))
{ {
finished=true; finished = true;
for (const auto& src: _sources) for (const auto& src : _sources)
{ {
if(src->GetPoolSize()) if (src->GetPoolSize())
{ {
vector<Pedestrian*> peds; vector<Pedestrian*> peds;
src->GenerateByFrequency(peds); src->GenerateByFrequency(peds);
AgentsQueue::Add(peds); AgentsQueue::Add(peds);
//ComputeBestPositionRandom(src.get(), peds);
ComputeBestPosition(src.get());
// compute the optimal position for insertion // 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->SetPos(Point(15,15),true);
//ped->Dump(ped->GetID());
} }
finished=false; finished = false;
//cout<<"Agents generated: "<<peds.size()<<endl; //cout<<"Agents generated: "<<peds.size()<<endl;
} }
//src->Dump(); //src->Dump();exit(0);
} }
_lastUpdateTime = current_time; _lastUpdateTime = current_time;
} }
//wait some time //wait some time
std::this_thread::sleep_for(std::chrono::milliseconds(10)); 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; auto dist = src->GetStartDistribution();
int subroomID=0; double bounds[4];
dist->Getbounds(bounds);
int roomID = dist->GetRoomId();
int subroomID = dist->GetSubroomID();
//Get all pedestrians in that location //Get all pedestrians in that location
vector<Pedestrian*> peds; 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 // compute the cells and cut with the bounds
const int count=peds.size(); const int count = peds.size();
float xValues[count]; float xValues[count];
float yValues[count]; float yValues[count];
for(int i=0;i<count;i++) for (int i = 0; i < count; i++)
{ {
xValues[i]=peds[i]->GetPos()._x; xValues[i] = peds[i]->GetPos()._x;
yValues[i]=peds[i]->GetPos()._y; yValues[i] = peds[i]->GetPos()._y;
} }
VoronoiDiagramGenerator vdg; 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.resetIterator();
vdg.resetVerticesIterator(); vdg.resetVerticesIterator();
float x1,y1;
printf("\n------vertices---------\n"); 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); //exit(0);
// float x1,y1,x2,y2; // float x1,y1,x2,y2;
//while(vdg.getNext(x1,y1,x2,y2)) //while(vdg.getNext(x1,y1,x2,y2))
...@@ -112,9 +181,77 @@ void AgentsSourcesManager::ComputeBestPosition(AgentsSource* src) ...@@ -112,9 +181,77 @@ void AgentsSourcesManager::ComputeBestPosition(AgentsSource* src)
// //
//} //}
//compute the best position //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) void AgentsSourcesManager::AddSource(std::shared_ptr<AgentsSource> src)
{ {
_sources.push_back(src); _sources.push_back(src);
...@@ -127,5 +264,5 @@ const std::vector<std::shared_ptr<AgentsSource> >& AgentsSourcesManager::GetSour ...@@ -127,5 +264,5 @@ const std::vector<std::shared_ptr<AgentsSource> >& AgentsSourcesManager::GetSour
void AgentsSourcesManager::SetBuilding(Building* building) void AgentsSourcesManager::SetBuilding(Building* building)
{ {
_building=building; _building = building;
} }
...@@ -14,6 +14,7 @@ ...@@ -14,6 +14,7 @@
//Forward declarations //Forward declarations
class AgentsSource; class AgentsSource;
class Building; class Building;
class Pedestrian;
class AgentsSourcesManager class AgentsSourcesManager
{ {
...@@ -61,7 +62,8 @@ private: ...@@ -61,7 +62,8 @@ private:
Building* _building=nullptr; Building* _building=nullptr;
private: private:
void ComputeBestPosition(AgentsSource* src); void ComputeBestPositionVoronoi(AgentsSource* src, Pedestrian* agent);
void ComputeBestPositionRandom(AgentsSource* src, std::vector<Pedestrian*>& peds);
}; };
#endif /* AGENTSSOURCESMANAGER_H_ */ #endif /* AGENTSSOURCESMANAGER_H_ */
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
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
*/
#ifndef VORONOI_DIAGRAM_GENERATOR
#define VORONOI_DIAGRAM_GENERATOR
#include <math.h>
#include <stdlib.h>
#include <string.h>
//#include "../logger/Logger.h"
#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 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
{
public:
VoronoiDiagramGenerator();
~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;
currentVertex++;
return true;
}
void reset();
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 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);