Commit 261eea18 authored by Mohcine Chraibi's avatar Mohcine Chraibi

minor changes

parent c1b55618
......@@ -272,7 +272,7 @@ set(source_files
pedestrian/StartDistribution.cpp
voronoi/VoronoiDiagramGenerator.cpp
# voronoi/VoronoiDiagramGenerator.cpp
voronoi-boost/VoronoiPositionGenerator.cpp
routing/ffRouter.cpp
......@@ -399,7 +399,7 @@ set(header_files
pedestrian/Pedestrian.h
pedestrian/AgentsSourcesManager.h
pedestrian/StartDistribution.h
voronoi/VoronoiDiagramGenerator.h
# voronoi/VoronoiDiagramGenerator.h
voronoi-boost/VoronoiPositionGenerator.h
mpi/LCGrid.h
......
......@@ -64,7 +64,7 @@ void HybridSimulationManager::Start()
bool HybridSimulationManager::Run(Simulation& sim)
{
Log->Write("WARN:\tJuPedSim requires the maximum number of pedestrians to be known at startup. However,\n"
Log->Write("WARNING:\tJuPedSim requires the maximum number of pedestrians to be known at startup. However,\n"
"\t\tin a hybrid approach it is not possible to know this number beforehand.\n"
"\t\tThis needs to be fixed in future. For the time being the number is arbitrarily set to 10,000.\n"
"\t\tThe simulation is likely to crash if the actual number of pedestrians exceeds 10,000.");
......
......@@ -2,7 +2,7 @@
* \file AgentsSourcesManager.cpp
* \date Apr 14, 2015
* \version v0.7
* \copyright <2009-2015> Forschungszentrum J��lich GmbH. All rights reserved.
* \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
......@@ -42,6 +42,7 @@
#include "AgentsQueue.h"
#include "../voronoi-boost/VoronoiPositionGenerator.h"
#define UNUSED(x) [&x]{}() // c++11 silence warnings
using namespace std;
......@@ -78,7 +79,7 @@ void AgentsSourcesManager::Run()
//it might be better to use a timer
_isCompleted = false;
bool finished = false;
long updateFrequency = 2; // @todo: get from inifile
long updateFrequency = 1;
do
{
int current_time = (int)Pedestrian::GetGlobalTime();
......@@ -123,6 +124,7 @@ bool AgentsSourcesManager::ProcessAllSources() const
void AgentsSourcesManager::ComputeBestPositionDummy(AgentsSource* src,
vector<Pedestrian*>& peds)const
{
UNUSED(src);
peds[0]->SetPos( Point(10,5.5) );
peds[1]->SetPos( Point(10,4.9) );
peds[2]->SetPos( Point(10,4.3) );
......
/*
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;
};
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);
out_bisector(newedge);
nedges += 1;
return(newedge);
}
//create a new site where the HalfEdges el1 and el2 intersect - note that the PointVDG 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 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-10<d && d<1.0e-10)
return ((struct Site *) NULL);
xint = (e1->c*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 PointVDG *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;
insertVertexAddress(nvertices,v);
nvertices += 1;
//out_vertex(v);
}
void VoronoiDiagramGenerator::deref(struct Site *v)
{
v -> refcnt -= 1;