Commit 7eaa59c7 authored by Ulrich Kemloh's avatar Ulrich Kemloh

Agents that could not be placed using vorovoi are now requeued

parent f9940660
......@@ -71,6 +71,11 @@ void AgentsSource::RemoveAgentsFromPool(std::vector<Pedestrian*>& ped, int count
}
}
void AgentsSource::AddAgentsToPool(std::vector<Pedestrian*>& peds)
{
_agents.insert(_agents.begin(),peds.begin(),peds.end());
}
int AgentsSource::GetPoolSize() const
{
return _agents.size();
......
......@@ -85,6 +85,13 @@ public:
*/
void RemoveAgentsFromPool(std::vector<Pedestrian*>& peds, int count);
/**
* Add the agents to the pool. This might be important in the case the removed agents could not
* be placed correctly. They can be requeued using this function.
* @param peds
*/
void AddAgentsToPool(std::vector<Pedestrian*>& peds);
/**
* @return the number of agents remaining
*/
......
......@@ -91,7 +91,7 @@ void AgentsSourcesManager::Run()
_lastUpdateTime = current_time;
}
//wait some time
// std::this_thread::sleep_for(std::chrono::milliseconds(1));
// std::this_thread::sleep_for(std::chrono::milliseconds(1));
} while (!finished);
Log->Write("INFO:\tTerminating agent manager thread");
_isCompleted = true;
......@@ -104,14 +104,14 @@ bool AgentsSourcesManager::ProcessAllSources() const
{
if (src->GetPoolSize())
{
vector<Pedestrian*> peds;
vector<Pedestrian*> peds;
src->RemoveAgentsFromPool(peds,src->GetFrequency());
Log->Write("INFO:\tSource %d generating %d agents (%d remaining)",src->GetId(),peds.size(),src->GetPoolSize());
//ComputeBestPositionRandom(src.get(), peds);
//todo: compute the optimal position for insertion using voronoi
if( !ComputeBestPositionVoronoiBoost(src.get(), peds, _building) )
Log->Write("INFO:\t there was no place for some pedestrians");
Log->Write("INFO:\t there was no place for some pedestrians");
//ComputeBestPositionTotalRandom(src.get(), peds );
//ComputeBestPositionDummy( src.get(), peds );
/*for (auto&& ped : peds)
......@@ -131,49 +131,49 @@ bool AgentsSourcesManager::ProcessAllSources() const
void AgentsSourcesManager::ComputeBestPositionDummy(AgentsSource* src,
vector<Pedestrian*>& peds)const
{
peds[0]->SetPos( Point(10,5.5) );
peds[1]->SetPos( Point(10,4.9) );
peds[2]->SetPos( Point(10,4.3) );
peds[3]->SetPos( Point(10,3.7) );
peds[0]->SetPos( Point(10,5.5) );
peds[1]->SetPos( Point(10,4.9) );
peds[2]->SetPos( Point(10,4.3) );
peds[3]->SetPos( Point(10,3.7) );
/*peds[0]->SetPos( Point(10,5.4) );
/*peds[0]->SetPos( Point(10,5.4) );
peds[1]->SetPos( Point(10,4.6) );
peds[2]->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);
}
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::ComputeBestPositionCompleteRandom(AgentsSource* src,
vector<Pedestrian*>& peds)const
vector<Pedestrian*>& peds)const
{
auto dist = src->GetStartDistribution();
auto subroom = _building->GetRoom(dist->GetRoomId())->GetSubRoom(dist->GetSubroomID());
vector<Point> positions = PedDistributor::PossiblePositions(*subroom);
srand (time(NULL));
for (auto& ped : peds)
{
if( positions.size() )
{
int index = rand()%positions.size();
Point new_pos = positions[index];
positions.erase(positions.begin() + index);
ped->SetPos(new_pos, true);
AdjustVelocityByNeighbour(ped);
}
else
{
Log->Write("\t No place for a pedestrian");
break;
}
}
auto dist = src->GetStartDistribution();
auto subroom = _building->GetRoom(dist->GetRoomId())->GetSubRoom(dist->GetSubroomID());
vector<Point> positions = PedDistributor::PossiblePositions(*subroom);
srand (time(NULL));
for (auto& ped : peds)
{
if( positions.size() )
{
int index = rand()%positions.size();
Point new_pos = positions[index];
positions.erase(positions.begin() + index);
ped->SetPos(new_pos, true);
AdjustVelocityByNeighbour(ped);
}
else
{
Log->Write("\t No place for a pedestrian");
break;
}
}
}
......@@ -294,7 +294,7 @@ void AgentsSourcesManager::ComputeBestPositionVoronoi(AgentsSource* src,
//compute the best position
//exit(0);
}
*/
*/
......
......@@ -39,20 +39,20 @@ using boost::polygon::high;
//wrapping the boost objects
namespace boost {
namespace polygon {
template <>
struct geometry_concept<Point> {
typedef point_concept type;
};
template <>
struct point_traits<Point> {
typedef int coordinate_type;
static inline coordinate_type get(
const Point& point, orientation_2d orient) {
return (orient == HORIZONTAL) ? point._x : point._y;
}
};
template <>
struct geometry_concept<Point> {
typedef point_concept type;
};
template <>
struct point_traits<Point> {
typedef int coordinate_type;
static inline coordinate_type get(
const Point& point, orientation_2d orient) {
return (orient == HORIZONTAL) ? point._x : point._y;
}
};
} // polygon
} // boost
......@@ -61,160 +61,174 @@ using namespace std;
//functions
//TODO: refactor the function
bool IsEnoughInSubroom( SubRoom* subroom, Point& pt )
{
std::vector<Wall> walls = subroom->GetAllWalls();
std::vector<Transition*> transitions= subroom->GetAllTransitions();
std::vector<Crossing*> crossings = subroom->GetAllCrossings();
double radius = 0.4; //radius of a person
//checking distance to walls
for(unsigned int i=0; i<walls.size(); i++)
if ( walls[i].DistTo(pt) < radius )
return false;
//checking distance to transitions
for(unsigned int i=0; i<transitions.size(); i++)
if ( transitions[i]->DistTo(pt) < radius )
return false;
//checking distance to crossings
for(unsigned int i=0; i<crossings.size(); i++)
if ( crossings[i]->DistTo(pt) < radius )
return false;
return true;
std::vector<Transition*> transitions= subroom->GetAllTransitions();
std::vector<Crossing*> crossings = subroom->GetAllCrossings();
double radius = 0.4; //radius of a person
//checking distance to walls
//std::vector<Wall> walls = subroom->GetAllWalls();
// for(unsigned int i=0; i<walls.size(); i++)
// if ( walls[i].DistTo(pt) < radius )
// return false;
//TODO: code refactoring:
for (const auto& wall: subroom->GetAllWalls())
if(wall.DistTo(pt)<radius)
return false;
//checking distance to transitions
for(unsigned int i=0; i<transitions.size(); i++)
if ( transitions[i]->DistTo(pt) < radius )
return false;
//checking distance to crossings
for(unsigned int i=0; i<crossings.size(); i++)
if ( crossings[i]->DistTo(pt) < radius )
return false;
return true;
}
bool ComputeBestPositionVoronoiBoost(AgentsSource* src,
std::vector<Pedestrian*>& peds, Building* building)
bool ComputeBestPositionVoronoiBoost(AgentsSource* src, std::vector<Pedestrian*>& peds,
Building* building)
{
bool return_value = true;
auto dist = src->GetStartDistribution();
int roomID = dist->GetRoomId();
int subroomID = dist->GetSubroomID();
std::string caption = (building->GetRoom( roomID ))->GetCaption();
double radius = 0.4; //DO: change! radius of a person
std::vector<Pedestrian*> existing_peds;
building->GetPedestrians(roomID, subroomID, existing_peds);
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 )
{
Point center_pos = subroom->GetCentroid();
srand (time(NULL));
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);
Point new_pos = center_pos + random_pos;
if ( subroom->IsInSubRoom( new_pos ) )
{
if( IsEnoughInSubroom(subroom, new_pos ) )
{
ped->SetPos(center_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(center_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(center_pos, true);
Point v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
double speed=ped->GetV0Norm();
v=v*speed;
ped->SetV(v);
}
}//0
else
{
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)
{
const Point& pos = (*iter)->GetPos();
temp.SetX( (int)( pos.GetX()*factor ) );
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?
}
//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;
VoronoiBestVertexMax(discrete_positions, vd, subroom, factor, chosen_it, dis);
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
{
return_value = false;
//reject the pedestrian
}
/*else //try with the maximum distance, don't need this if already using the VoronoiBestVertexMax function
bool return_value = true;
auto dist = src->GetStartDistribution();
int roomID = dist->GetRoomId();
int subroomID = dist->GetSubroomID();
std::string caption = (building->GetRoom( roomID ))->GetCaption();
double radius = 0.4; //DO: change! radius of a person
std::vector<Pedestrian*> existing_peds;
std::vector<Pedestrian*> peds_without_place;
building->GetPedestrians(roomID, subroomID, existing_peds);
SubRoom* subroom = building->GetRoom( roomID )->GetSubRoom(subroomID);
const 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++ )
{
const 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 );
}
std::vector<Pedestrian*>::iterator iter_ped;
for (iter_ped = peds.begin(); iter_ped != peds.end(); )
{
Pedestrian* ped = (*iter_ped);
if(existing_peds.size() == 0 )
{
const Point& center_pos = subroom->GetCentroid();
// we have already initiate a random number generator using the seed from the ini file
//srand (time(NULL));
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);
Point new_pos = center_pos + random_pos;
if ( subroom->IsInSubRoom( new_pos ) )
{
if( IsEnoughInSubroom(subroom, new_pos ) )
{
ped->SetPos(center_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(center_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(center_pos, true);
Point v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
double speed=ped->GetV0Norm();
v=v*speed;
ped->SetV(v);
}
}//0
else
{
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)
{
const Point& pos = (*iter)->GetPos();
temp.SetX( (int)( pos.GetX()*factor ) );
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?
}
//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;
VoronoiBestVertexMax(discrete_positions, vd, subroom, factor, chosen_it, dis);
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 );
// proceed to the next pedestrian
++iter_ped;
}
else
{
//reject the pedestrian:
// remove from the initial vector since it should only contain the pedestrians that could
// find a place. Put in a different queue, they will be put back in the source.
return_value = false;
peds_without_place.push_back(*iter_ped);
iter_ped=peds.erase(iter_ped);
}
/*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
......@@ -229,154 +243,159 @@ bool ComputeBestPositionVoronoiBoost(AgentsSource* src,
//reject the pedestrian
}
}*/
}// >0
}// >0
existing_peds.push_back(ped);
}//for loop
existing_peds.push_back(ped);
}//for loop
return return_value;
//maybe not all pedestrians could find a place, requeue them in the source
if(peds_without_place.size()>0)
src->AddAgentsToPool(peds_without_place);
return return_value;
}
//gives an agent the mean velocity of his voronoi-neighbors
void VoronoiAdjustVelocityNeighbour( const voronoi_diagram<double>& vd, voronoi_diagram<double>::const_vertex_iterator& chosen_it,
Pedestrian* ped, const std::vector<Point>& velocities_vector )
Pedestrian* ped, const std::vector<Point>& velocities_vector )
{
//finding the neighbors (nearest pedestrians) of the chosen vertex
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;
double no=0;
std::size_t index;
do
{
no++;
index = ( edge->cell() )->source_index();
const Point& v = velocities_vector[index];
sum_x += v.GetX();
sum_y += v.GetY();
edge = edge->rot_next();
} while (edge != vertex.incident_edge());
Point v(sum_x/no, sum_y/no);
ped->SetV(v);
//finding the neighbors (nearest pedestrians) of the chosen vertex
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;
double no=0;
std::size_t index;
do
{
no++;
index = ( edge->cell() )->source_index();
const Point& v = velocities_vector[index];
sum_x += v.GetX();
sum_y += v.GetY();
edge = edge->rot_next();
} while (edge != vertex.incident_edge());
Point v(sum_x/no, sum_y/no);
ped->SetV(v);
}
//gives the voronoi vertex with max distance
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 )
double factor, voronoi_diagram<double>::const_vertex_iterator& max_it, double& max_dis )
{
double dis = 0;
for (voronoi_diagram<double>::const_vertex_iterator it = vd.vertices().begin(); it != vd.vertices().end(); ++it)
{
Point vert_pos( it->x()/factor, it->y()/factor );
if( subroom->IsInSubRoom(vert_pos) )
if( IsEnoughInSubroom( subroom, vert_pos ) )
{
const voronoi_diagram<double>::vertex_type &vertex = *it;
const voronoi_diagram<double>::edge_type *edge = vertex.incident_edge();
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)
{
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
double dis = 0;
for (voronoi_diagram<double>::const_vertex_iterator it = vd.vertices().begin(); it != vd.vertices().end(); ++it)
{
Point vert_pos( it->x()/factor, it->y()/factor );
if( subroom->IsInSubRoom(vert_pos) )
if( IsEnoughInSubroom( subroom, vert_pos ) )
{
const voronoi_diagram<double>::vertex_type &vertex = *it;
const voronoi_diagram<double>::edge_type *edge = vertex.incident_edge();
std::size_t index = ( edge->cell() )->source_index();
Point p = discrete_positions[index];
dis = ( p.GetX() - it->x() )*( p.GetX