Maintenance at Wednesday, 30. June 2021, from 8:15 to 9:15
Some of the planned changes may require user action

Commit 58473574 authored by Andrijana Brkic's avatar Andrijana Brkic
Browse files

Almost final version of the function, everything working

parent a379e59e
......@@ -116,7 +116,7 @@ void AgentsSourcesManager::Run()
//it might be better to use a timer
_isCompleted = false;
bool finished = false;
long updateFrequency = 5; // 1 second
long updateFrequency = 3; // 1 = second
do
{
int current_time = Pedestrian::GetGlobalTime();
......@@ -155,6 +155,7 @@ bool AgentsSourcesManager::ProcessAllSources() const
//ComputeBestPositionRandom(src.get(), peds);
//todo: compute the optimal position for insertion using voronoi
ComputeBestPositionVoronoiBoost2(src.get(), peds);
//ComputeBestPositionDummy( src.get(), peds );
/*for (auto&& ped : peds)
{
ComputeBestPositionVoronoiBoost(src.get(), ped);
......@@ -167,6 +168,24 @@ bool AgentsSourcesManager::ProcessAllSources() const
}
return empty;
}
//4 agents frequency, just for an example
void AgentsSourcesManager::ComputeBestPositionDummy(AgentsSource* src,
vector<Pedestrian*>& peds)const
{
peds[0]->SetPos( Point(10,5.4) );
peds[1]->SetPos( Point(10,4.8) );
peds[2]->SetPos( Point(10,4.4) );
peds[3]->SetPos( Point(10,3.8) );
for(auto&& ped : peds)
{
Point v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
double speed=ped->GetV0Norm();
v=v*speed;
ped->SetV(v);
}
}
/*
void AgentsSourcesManager::ComputeBestPositionVoronoi(AgentsSource* src,
Pedestrian* agent) const
......@@ -298,108 +317,66 @@ void AgentsSourcesManager::ComputeBestPositionVoronoiBoost2(AgentsSource* src,
std::vector<Pedestrian*> existing_peds;
_building->GetPedestrians(roomID, subroomID, existing_peds);
SubRoom* subroom=(_building->GetRoom( roomID ))->GetSubRoom(subroomID);
//subroom->SetHelpVariables();
SubRoom* subroom = (_building->GetRoom( roomID ))->GetSubRoom(subroomID);
vector<Point> room_vertices = subroom->GetPolygon();
double factor = 100; //factor for conversion to integer for the boost voronoi
vector<Point> fake_peds; //doing this now so I don't have to do it in every loop
Point temp(0,0);
for (unsigned int i=0; i<room_vertices.size(); i++ )
{
Point center_pos = subroom->GetCentroid();
temp.SetX( center_pos.GetX()-room_vertices[i].GetX( ) );
temp.SetY( center_pos.GetY()-room_vertices[i].GetY( ) );
temp = temp/sqrt(temp.NormSquare());
temp = temp*(radius*1.4); //now the norm of the vector is ~r*sqrt(2), pointing to the center
temp = temp + room_vertices[i];
temp.SetX( (int)(temp.GetX()*factor) );
temp.SetY( (int)(temp.GetY()*factor) );
fake_peds.push_back( temp );
}
for(auto&& ped : peds)
{
if(existing_peds.size() == 0 )
{
Log->Write("INFO:\tenter case 0");
Point new_pos = subroom->GetCentroid();
ped->SetPos(new_pos, true);
Point v =(ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
double speed=ped->GetV0Norm();
v=v*speed;
ped->SetV(v);
}//0
else if(existing_peds.size() == 1 )
{
Log->Write("INFO:\tenter case 1");
Point p = existing_peds[0]->GetPos();
vector<Point> room_vertices = subroom->GetPolygon();
Point new_pos = subroom->GetCentroid();
srand (time(NULL));
unsigned int i = rand() % room_vertices.size();
double dis = ( p.GetX() - room_vertices[i].GetX() ) * ( p.GetX() - room_vertices[i].GetX() )
+ ( p.GetY() - room_vertices[i].GetY() ) * ( p.GetY() - room_vertices[i].GetY() );
if( dis > radius*radius)
double x_coor = 3 * ( (double)rand() / (double)RAND_MAX ) - 1.5;
double y_coor = 3 * ( (double)rand() / (double)RAND_MAX ) - 1.5;
Point random_pos(x_coor, y_coor);
if ( subroom->IsInsideOfPolygonHelp( new_pos + random_pos ) )
{
Point new_pos( p.GetX()/2.0 + room_vertices[i].GetX()/2.0, p.GetY()/2.0 + room_vertices[i].GetY()/2.0 );
ped->SetPos( new_pos, true );
Point v =(ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
ped->SetPos(new_pos + random_pos, true);
Point v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
double speed=ped->GetV0Norm();
v=v*speed;
ped->SetV(v);
}
else
{
ped->SetPos(new_pos, true);
Point v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
double speed=ped->GetV0Norm();
v=v*speed;
ped->SetV(v);
}
}//1
else if(existing_peds.size() == 2)
{
Log->Write("INFO:\tenter case 2");
Point p1 = existing_peds[0]->GetPos();
Point p2 = existing_peds[1]->GetPos();
double x1 = p1.GetX(); double y1 = p1.GetY();
double x2 = p2.GetX(); double y2 = p2.GetY();
Point t1( (x1+x2)/2.0 + 1.1*(y1-y2), (y1+y2)/2.0 + 1.1*(x2-x1) );
Point t2( (x1+x2)/2.0 + 1.1*(y2-y1), (y1+y2)/2.0 + 1.1*(x1-x2) );
for (double alpha = 1.0; alpha>0 ; alpha-=0.1)
if( subroom->IsInsideOfPolygonHelp(t1) )
{
ped->SetPos(t1, true );
Point new_velo(-1,0);
ped->SetV( new_velo );
break;
}
else if( subroom->IsInsideOfPolygonHelp(t1) )
{
ped->SetPos(t2, true );
Point new_velo(-1,0);
ped->SetV( new_velo );
break;
}
else
{
t1.SetX( (x1+x2)/2.0 + alpha*(y1-y2) );
t1.SetY( (y1+y2)/2.0 + alpha*(x2-x1) );
t2.SetX( (x1+x2)/2.0 + alpha*(y2-y1) );
t2.SetY( (y1+y2)/2.0 + alpha*(x1-x2) );
}
//in case it didn't work
for (double alpha = 1.0; alpha<3 ; alpha+=0.2)
if( subroom->IsInsideOfPolygonHelp(t1) )
{
ped->SetPos(t1, true );
ped->SetV( ped->GetV0() );
break;
}
else if( subroom->IsInsideOfPolygonHelp(t1) )
{
ped->SetPos(t2, true );
ped->SetV( ped->GetV0() );
break;
}
else
{
t1.SetX( (x1+x2)/2.0 + alpha*(y1-y2) );
t1.SetY( (y1+y2)/2.0 + alpha*(x2-x1) );
t2.SetX( (x1+x2)/2.0 + alpha*(y2-y1) );
t2.SetY( (y1+y2)/2.0 + alpha*(x1-x2) );
}
}//2
}//0
else
{
Log->Write("INFO:\tenter case 3");
double factor = 100; //factor for conversion to integer
std::vector<Point> discrete_positions;
std::vector<Point> velocities_vector;
Point temp(0,0);
Point v(0,0);
double no = 0;
//points from double to integer
for (auto&& iter = existing_peds.begin(); iter != existing_peds.end(); ++iter)
{
......@@ -408,27 +385,38 @@ void AgentsSourcesManager::ComputeBestPositionVoronoiBoost2(AgentsSource* src,
temp.SetY( (int)( pos.GetY()*factor ) );
discrete_positions.push_back( temp );
velocities_vector.push_back( (*iter)->GetV() );
//calculating the mean, using it for the fake pedestrians
v = v + (*iter)->GetV();
no++;
}
v = v/no; //this is the mean of all the velocities
//adding fake people to the voronoi diagram
for (unsigned int i=0; i<room_vertices.size(); i++ )
{
discrete_positions.push_back( fake_peds[i] );
velocities_vector.push_back( v ); //DO: what speed?
}
//subroom->AddFakePeople( vector<Point>& discrete_positions, vector<Point>& velocities_vector,v);
//constructing the diagram
voronoi_diagram<double> vd;
construct_voronoi(discrete_positions.begin(), discrete_positions.end(), &vd);
voronoi_diagram<double>::const_vertex_iterator chosen_it=vd.vertices().begin();
double dis=0;
VoronoiBestVertexRandMax(discrete_positions, vd, subroom, factor, chosen_it, dis ); //dis is the squared distance!
//VoronoiBestVertexRand(discrete_positions, vd, subroom, factor, chosen_it, dis );
//VoronoiBestVertexMax(discrete_positions, vd, subroom, factor, chosen_it, dis );
voronoi_diagram<double>::const_vertex_iterator chosen_it = vd.vertices().begin();
double dis = 0;
VoronoiBestVertexMax(discrete_positions, vd, subroom, factor, chosen_it, dis);
//giving the position and velocity
if( dis > radius*factor*radius*factor)// be careful with the factor!! radius*factor
{
Point pos( chosen_it->x()/factor, chosen_it->y()/factor ); //check!
ped->SetPos(pos , true);
VoronoiAdjustVelocityNeighbour( vd, chosen_it, ped, velocities_vector );
}
else //try with the maximum distance
/*else //try with the maximum distance, don't need this if already using the VoronoiBestVertexMax function
{
VoronoiBestVertexMax(discrete_positions, vd, subroom, factor, chosen_it, dis );
if( dis > radius*factor*radius*factor)// be careful with the factor!! radius*factor
......@@ -437,8 +425,8 @@ void AgentsSourcesManager::ComputeBestPositionVoronoiBoost2(AgentsSource* src,
ped->SetPos(pos , true);
VoronoiAdjustVelocityNeighbour( vd, chosen_it, ped, velocities_vector );
}
}
}//3
}*/
}// >0
existing_peds.push_back(ped);
}//for loop
......@@ -452,7 +440,7 @@ void AgentsSourcesManager::VoronoiAdjustVelocityNeighbour( const voronoi_diagram
const voronoi_diagram<double>::vertex_type &vertex = *chosen_it;
const voronoi_diagram<double>::edge_type *edge = vertex.incident_edge();
double sum_x=0, sum_y=0;
int no=0;
double no=0;
std::size_t index;
do
......@@ -471,13 +459,13 @@ void AgentsSourcesManager::VoronoiAdjustVelocityNeighbour( const voronoi_diagram
}
//gives the voronoi vertex with max distance
void AgentsSourcesManager::VoronoiBestVertexMax (const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom, int factor,
voronoi_diagram<double>::const_vertex_iterator& max_it, double& max_dis ) const
void AgentsSourcesManager::VoronoiBestVertexMax (const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom,
double factor, voronoi_diagram<double>::const_vertex_iterator& max_it, double& max_dis ) const
{
double dis = 0;
for (voronoi_diagram<double>::const_vertex_iterator it = vd.vertices().begin(); it != vd.vertices().end(); ++it)
{
Point vert_pos = Point( it->x()/factor, it->y()/factor );
Point vert_pos( it->x()/factor, it->y()/factor );
if( subroom->IsInsideOfPolygonHelp(vert_pos) ) //wrote this function by myself
{
const voronoi_diagram<double>::vertex_type &vertex = *it;
......@@ -486,19 +474,20 @@ void AgentsSourcesManager::VoronoiBestVertexMax (const std::vector<Point>& discr
std::size_t index = ( edge->cell() )->source_index();
Point p = discrete_positions[index];
dis = ( p.GetX() - it->x() )*( p.GetX() - it->x() ) + ( p.GetY() - it->y() )*( p.GetY() - it->y() ) ;
if(dis>max_dis)
dis = ( p.GetX() - it->x() )*( p.GetX() - it->x() ) + ( p.GetY() - it->y() )*( p.GetY() - it->y() ) ;
if(dis > max_dis)
{
max_dis = dis;
max_it = it;
}
}
}
//at the end, max_it is the choosen vertex, or the first vertex - max_dis=0 assures that this position will not be taken
}
//gives random voronoi vertex but with weights proportional to squared distances
void AgentsSourcesManager::VoronoiBestVertexRandMax (const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom, int factor,
voronoi_diagram<double>::const_vertex_iterator& chosen_it, double& dis ) const
void AgentsSourcesManager::VoronoiBestVertexRandMax (const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom,
double factor, voronoi_diagram<double>::const_vertex_iterator& chosen_it, double& dis ) const
{
std::vector< voronoi_diagram<double>::const_vertex_iterator > possible_vertices;
vector<double> partial_sums;
......@@ -551,8 +540,8 @@ void AgentsSourcesManager::VoronoiBestVertexRandMax (const std::vector<Point>& d
}
//gives a random voronoi vertex
void AgentsSourcesManager::VoronoiBestVertexRand (const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom, int factor,
voronoi_diagram<double>::const_vertex_iterator& chosen_it, double& dis ) const
void AgentsSourcesManager::VoronoiBestVertexRand (const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom,
double factor, voronoi_diagram<double>::const_vertex_iterator& chosen_it, double& dis ) const
{
std::vector< voronoi_diagram<double>::const_vertex_iterator > possible_vertices;
std::vector<double> distances;
......
......@@ -148,22 +148,24 @@ private:
*/
void ComputeBestPositionVoronoiBoost2(AgentsSource* src, std::vector<Pedestrian*>& peds) const;
void ComputeBestPositionDummy(AgentsSource* src, std::vector<Pedestrian*>& peds) const;
/**Position incoming pedestrian on the vertex with the greatest distance
*
*/
void VoronoiBestVertexMax (const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom, int factor,
void VoronoiBestVertexMax (const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom, double factor,
voronoi_diagram<double>::const_vertex_iterator& max_it, double& max_dis ) const ;
/**Position incoming pedestrian on the vertex with greater probability for greater distances
*
*/
void VoronoiBestVertexRandMax (const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom, int factor,
void VoronoiBestVertexRandMax (const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom, double factor,
voronoi_diagram<double>::const_vertex_iterator& max_it, double& max_dis ) const ;
/**Position incoming pedestrian on a random vertex
*
*/
void VoronoiBestVertexRand (const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom, int factor,
void VoronoiBestVertexRand (const std::vector<Point>& discrete_positions, const voronoi_diagram<double>& vd, SubRoom* subroom, double factor,
voronoi_diagram<double>::const_vertex_iterator& max_it, double& max_dis ) const ;
void VoronoiAdjustVelocityNeighbour( const voronoi_diagram<double>& vd, voronoi_diagram<double>::const_vertex_iterator& chosen_it,
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment