Commit 3e9a3fb0 authored by Mohcine Chraibi's avatar Mohcine Chraibi

WIP: Adress issue #158

parent f45cd63c
...@@ -78,7 +78,7 @@ void AgentsSourcesManager::Run() ...@@ -78,7 +78,7 @@ void AgentsSourcesManager::Run()
int current_time = (int)Pedestrian::GetGlobalTime(); int current_time = (int)Pedestrian::GetGlobalTime();
if ((current_time != _lastUpdateTime) if ((current_time != _lastUpdateTime)
&& ((current_time % updateFrequency) == 0)) && ((current_time % updateFrequency) == 0))
{ {
finished=ProcessAllSources(); finished=ProcessAllSources();
_lastUpdateTime = current_time; _lastUpdateTime = current_time;
} }
...@@ -93,14 +93,15 @@ bool AgentsSourcesManager::ProcessAllSources() const ...@@ -93,14 +93,15 @@ bool AgentsSourcesManager::ProcessAllSources() const
{ {
bool empty=true; bool empty=true;
double current_time = Pedestrian::GetGlobalTime(); double current_time = Pedestrian::GetGlobalTime();
// std::cout << "\n -- current time: " << current_time << " number " << _building->GetAllPedestrians().size() << std::endl;
for (const auto& src : _sources) for (const auto& src : _sources)
{ {
std::cout << "src: " << src->GetId() << " -- current time: " << current_time << " number of peds " << _building->GetAllPedestrians().size() << std::endl;
if (src->GetPoolSize() && (src->GetPlanTime() <= current_time) )// maybe diff<eps if (src->GetPoolSize() && (src->GetPlanTime() <= current_time) )// maybe diff<eps
{ {
vector<Pedestrian*> peds; vector<Pedestrian*> peds;
src->RemoveAgentsFromPool(peds,src->GetFrequency()); src->RemoveAgentsFromPool(peds,src->GetFrequency());
Log->Write("INFO:\tSource %d generating %d agents (%d remaining)",src->GetId(),peds.size(),src->GetPoolSize()); Log->Write("> INFO:\tSource %d generating %d agents (%d remaining)\n",src->GetId(),peds.size(),src->GetPoolSize());
//ComputeBestPositionRandom(src.get(), peds); //ComputeBestPositionRandom(src.get(), peds);
//todo: here every pedestrian needs an exitline //todo: here every pedestrian needs an exitline
...@@ -109,7 +110,7 @@ bool AgentsSourcesManager::ProcessAllSources() const ...@@ -109,7 +110,7 @@ bool AgentsSourcesManager::ProcessAllSources() const
AgentsQueueIn::Add(peds); AgentsQueueIn::Add(peds);
empty = false; empty = false;
//src->Dump(); src->Dump();
} }
if (src->GetPlanTime() > current_time) // for the case we still expect if (src->GetPlanTime() > current_time) // for the case we still expect
// agents coming // agents coming
...@@ -123,15 +124,15 @@ bool AgentsSourcesManager::ProcessAllSources() const ...@@ -123,15 +124,15 @@ bool AgentsSourcesManager::ProcessAllSources() const
void AgentsSourcesManager::ComputeBestPositionDummy(AgentsSource* src, void AgentsSourcesManager::ComputeBestPositionDummy(AgentsSource* src,
vector<Pedestrian*>& peds)const vector<Pedestrian*>& peds)const
{ {
UNUSED(src); UNUSED(src);
peds[0]->SetPos( Point(10,5.5) ); peds[0]->SetPos( Point(10,5.5) );
peds[1]->SetPos( Point(10,4.9) ); peds[1]->SetPos( Point(10,4.9) );
peds[2]->SetPos( Point(10,4.3) ); peds[2]->SetPos( Point(10,4.3) );
peds[3]->SetPos( Point(10,3.7) ); 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[1]->SetPos( Point(10,4.6) );
peds[2]->SetPos( Point(10,3.8) );*/ peds[2]->SetPos( Point(10,3.8) );*/
for(auto&& ped : peds) for(auto&& ped : peds)
{ {
...@@ -148,9 +149,11 @@ void AgentsSourcesManager::ComputeBestPositionCompleteRandom(AgentsSource* src, ...@@ -148,9 +149,11 @@ void AgentsSourcesManager::ComputeBestPositionCompleteRandom(AgentsSource* src,
auto dist = src->GetStartDistribution(); auto dist = src->GetStartDistribution();
auto subroom = _building->GetRoom(dist->GetRoomId())->GetSubRoom(dist->GetSubroomID()); auto subroom = _building->GetRoom(dist->GetRoomId())->GetSubRoom(dist->GetSubroomID());
vector<Point> positions = PedDistributor::PossiblePositions(*subroom); vector<Point> positions = PedDistributor::PossiblePositions(*subroom);
double seed = time(NULL);
//TODO: get the seed from the simulation //TODO: get the seed from the simulation
srand (time(NULL)); std:: cout << "seed: "<< seed << std::endl;
srand (seed);
for (auto& ped : peds) for (auto& ped : peds)
{ {
...@@ -160,6 +163,8 @@ void AgentsSourcesManager::ComputeBestPositionCompleteRandom(AgentsSource* src, ...@@ -160,6 +163,8 @@ void AgentsSourcesManager::ComputeBestPositionCompleteRandom(AgentsSource* src,
Point new_pos = positions[index]; Point new_pos = positions[index];
positions.erase(positions.begin() + index); positions.erase(positions.begin() + index);
ped->SetPos(new_pos, true); ped->SetPos(new_pos, true);
std:: cout << "pos: " << new_pos._x << new_pos._y << std::endl;
AdjustVelocityByNeighbour(ped); AdjustVelocityByNeighbour(ped);
} }
else else
...@@ -328,25 +333,25 @@ void AgentsSourcesManager::ComputeBestPositionRandom(AgentsSource* src, ...@@ -328,25 +333,25 @@ void AgentsSourcesManager::ComputeBestPositionRandom(AgentsSource* src,
&& (bounds[2] <= pos._y) && (pos._y < bounds[3])) && (bounds[2] <= pos._y) && (pos._y < bounds[3]))
{ {
bool enough_space = true; bool enough_space = true;
//checking enough space!! //checking enough space!!
vector<Pedestrian*> neighbours; vector<Pedestrian*> neighbours;
_building->GetGrid()->GetNeighbourhood(pos,neighbours); _building->GetGrid()->GetNeighbourhood(pos,neighbours);
for (const auto& ngh: neighbours) for (const auto& ngh: neighbours)
if( (ngh->GetPos() - pos).NormSquare() < 4*radius*radius ) if( (ngh->GetPos() - pos).NormSquare() < 4*radius*radius )
{ {
enough_space = false; enough_space = false;
break; break;
} }
if( enough_space ) if( enough_space )
{ {
index = a; index = a;
break; break;
} }
} }
} }
...@@ -369,16 +374,16 @@ void AgentsSourcesManager::ComputeBestPositionRandom(AgentsSource* src, ...@@ -369,16 +374,16 @@ void AgentsSourcesManager::ComputeBestPositionRandom(AgentsSource* src,
{ {
const Point& pos = positions[index]; const Point& pos = positions[index];
extra_positions.push_back(pos); extra_positions.push_back(pos);
(*iter_ped)->SetPos(pos, true); //true for the initial position (*iter_ped)->SetPos(pos, true); //true for the initial position
positions.erase(positions.begin() + index); positions.erase(positions.begin() + index);
//at this point we have a position //at this point we have a position
//so we can adjust the velocity //so we can adjust the velocity
//AdjustVelocityUsingWeidmann(ped); //AdjustVelocityUsingWeidmann(ped);
AdjustVelocityByNeighbour( (*iter_ped) ); AdjustVelocityByNeighbour( (*iter_ped) );
//move iterator //move iterator
iter_ped++; iter_ped++;
} }
...@@ -421,7 +426,7 @@ void AgentsSourcesManager::AdjustVelocityByNeighbour(Pedestrian* ped) const ...@@ -421,7 +426,7 @@ void AgentsSourcesManager::AdjustVelocityByNeighbour(Pedestrian* ped) const
//mean speed //mean speed
if(count==0) if(count==0)
{ {
speed=ped->GetEllipse().GetV0(); // FIXME: bad fix for: peds without navline (ar.graf) speed=ped->GetEllipse().GetV0(); // FIXME: bad fix for: peds without navline (ar.graf)
//speed=ped->GetV0Norm(); //speed=ped->GetV0Norm();
} }
else else
...@@ -438,7 +443,7 @@ void AgentsSourcesManager::AdjustVelocityByNeighbour(Pedestrian* ped) const ...@@ -438,7 +443,7 @@ void AgentsSourcesManager::AdjustVelocityByNeighbour(Pedestrian* ped) const
} }
else else
{ {
Log->Write("ERROR:\t no route could be found for agent [%d] going to [%d]",ped->GetID(),ped->GetFinalDestination()); Log->Write(">> ERROR:\t no route could be found for agent [%d] going to [%d]",ped->GetID(),ped->GetFinalDestination());
//that will be most probably be fixed in the next computation step. //that will be most probably be fixed in the next computation step.
// so do not abort // so do not abort
} }
...@@ -485,7 +490,7 @@ void AgentsSourcesManager::AdjustVelocityUsingWeidmann(Pedestrian* ped) const ...@@ -485,7 +490,7 @@ void AgentsSourcesManager::AdjustVelocityUsingWeidmann(Pedestrian* ped) const
} }
else else
{ {
Log->Write("ERROR:\t no route could be found for agent [%d] going to [%d]",ped->GetID(),ped->GetFinalDestination()); Log->Write(">>> ERROR:\t no route could be found for agent [%d] going to [%d]",ped->GetID(),ped->GetFinalDestination());
//that will be most probably be fixed in the next computation step. //that will be most probably be fixed in the next computation step.
// so do not abort // so do not abort
} }
...@@ -508,7 +513,7 @@ void AgentsSourcesManager::SortPositionByDensity(std::vector<Point>& positions, ...@@ -508,7 +513,7 @@ void AgentsSourcesManager::SortPositionByDensity(std::vector<Point>& positions,
for(const auto& p: neighbours) for(const auto& p: neighbours)
{ {
//FIXME: p can be null, if deleted in the main simulation thread. //FIXME: p can be null, if deleted in the main simulation thread.
if( p && (pt-p->GetPos()).NormSquare()<=radius_square) if( p && (pt-p->GetPos()).NormSquare()<=radius_square)
density+=1.0; density+=1.0;
} }
......
...@@ -74,12 +74,12 @@ bool IsEnoughInSubroom( SubRoom* subroom, Point& pt, double radius ) ...@@ -74,12 +74,12 @@ bool IsEnoughInSubroom( SubRoom* subroom, Point& pt, double radius )
return false; return false;
for(const auto& trans: subroom->GetAllTransitions() ) for(const auto& trans: subroom->GetAllTransitions() )
if ( trans->DistTo(pt) < radius + 0.1 ) if ( trans->DistTo(pt) < radius + 0.1 )
return false; return false;
for( const auto& cross: subroom->GetAllCrossings() ) for( const auto& cross: subroom->GetAllCrossings() )
if( cross->DistTo(pt) < radius + 0.1 ) if( cross->DistTo(pt) < radius + 0.1 )
return false; return false;
return true; return true;
} }
...@@ -96,6 +96,8 @@ bool ComputeBestPositionVoronoiBoost(AgentsSource* src, std::vector<Pedestrian*> ...@@ -96,6 +96,8 @@ bool ComputeBestPositionVoronoiBoost(AgentsSource* src, std::vector<Pedestrian*>
std::vector<Pedestrian*> existing_peds; std::vector<Pedestrian*> existing_peds;
std::vector<Pedestrian*> peds_without_place; std::vector<Pedestrian*> peds_without_place;
building->GetPedestrians(roomID, subroomID, existing_peds); building->GetPedestrians(roomID, subroomID, existing_peds);
for (auto pp : existing_peds)
std::cout << "existing peds: " << pp->GetID() << " in " << pp->GetPos()._x << ", " << pp->GetPos()._y << std::endl;
double radius = 0.3; //radius of a person, 0.3 is just some number(needed for the fake_peds bellow), will be changed afterwards double radius = 0.3; //radius of a person, 0.3 is just some number(needed for the fake_peds bellow), will be changed afterwards
...@@ -145,7 +147,7 @@ bool ComputeBestPositionVoronoiBoost(AgentsSource* src, std::vector<Pedestrian*> ...@@ -145,7 +147,7 @@ bool ComputeBestPositionVoronoiBoost(AgentsSource* src, std::vector<Pedestrian*>
{ {
ped->SetPos(center_pos, true); ped->SetPos(center_pos, true);
} }
Point v; Point v;
if (ped->GetExitLine()) { if (ped->GetExitLine()) {
v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized(); v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
...@@ -162,8 +164,8 @@ bool ComputeBestPositionVoronoiBoost(AgentsSource* src, std::vector<Pedestrian*> ...@@ -162,8 +164,8 @@ bool ComputeBestPositionVoronoiBoost(AgentsSource* src, std::vector<Pedestrian*>
else //more than one pedestrian else //more than one pedestrian
{ {
//it would be better to maybe have a mapping between discrete_positions and pointers to the pedestrians //it would be better to maybe have a mapping between discrete_positions and pointers to the pedestrians
//then there would be no need to remember the velocities_vector and goal_vector //then there would be no need to remember the velocities_vector and goal_vector
std::vector<Point> discrete_positions; std::vector<Point> discrete_positions;
std::vector<Point> velocities_vector; std::vector<Point> velocities_vector;
std::vector<int> goal_vector; std::vector<int> goal_vector;
...@@ -210,7 +212,7 @@ bool ComputeBestPositionVoronoiBoost(AgentsSource* src, std::vector<Pedestrian*> ...@@ -210,7 +212,7 @@ bool ComputeBestPositionVoronoiBoost(AgentsSource* src, std::vector<Pedestrian*>
VoronoiBestVertexRandMax(discrete_positions, vd, subroom, factor, chosen_it, dis, radius); VoronoiBestVertexRandMax(discrete_positions, vd, subroom, factor, chosen_it, dis, radius);
else else
VoronoiBestVertexGreedy(discrete_positions, vd, subroom, factor, chosen_it, dis, radius); VoronoiBestVertexGreedy(discrete_positions, vd, subroom, factor, chosen_it, dis, radius);
if( dis > 4*radius*radius) if( dis > 4*radius*radius)
{ {
Point pos( chosen_it->x()/factor, chosen_it->y()/factor ); //check! Point pos( chosen_it->x()/factor, chosen_it->y()/factor ); //check!
...@@ -266,21 +268,21 @@ void VoronoiAdjustVelocityNeighbour(voronoi_diagram<double>::const_vertex_iterat ...@@ -266,21 +268,21 @@ void VoronoiAdjustVelocityNeighbour(voronoi_diagram<double>::const_vertex_iterat
std::size_t index = ( edge->cell() )->source_index(); std::size_t index = ( edge->cell() )->source_index();
if( ped->GetFinalDestination() == goal_vector[index] ) if( ped->GetFinalDestination() == goal_vector[index] )
{ {
no1++; no1++;
speed += velocities_vector[index].Norm(); speed += velocities_vector[index].Norm();
} }
else else
{ {
no2++; no2++;
backup_speed += velocities_vector[index].Norm(); backup_speed += velocities_vector[index].Norm();
} }
edge = edge->rot_next(); edge = edge->rot_next();
} while (edge != vertex.incident_edge()); } while (edge != vertex.incident_edge());
if(no1) if(no1)
speed = speed/no1; speed = speed/no1;
else else
speed = backup_speed/(no2*3.0); //just some small speed speed = backup_speed/(no2*3.0); //just some small speed
v = v*speed; v = v*speed;
ped->SetV(v); ped->SetV(v);
...@@ -324,26 +326,26 @@ void VoronoiAdjustVelocityNeighbour(voronoi_diagram<double>::const_vertex_iterat ...@@ -324,26 +326,26 @@ void VoronoiAdjustVelocityNeighbour(voronoi_diagram<double>::const_vertex_iterat
// //
// do // do
// { // {
// //do something // //do something
// if( goal_vector[index]!=-3 && goal_vector[index]!=ped->GetFinalDestination() ) // // if( goal_vector[index]!=-3 && goal_vector[index]!=ped->GetFinalDestination() ) //
// if( check_line.IntersectionWithCircle(p,1.0) ) //0.7 because the radius is around 0.3 // if( check_line.IntersectionWithCircle(p,1.0) ) //0.7 because the radius is around 0.3
// { // {
// score -= 100; // score -= 100;
// break; // break;
// } // }
// //
// //
// //change edge // //change edge
// edge = edge->rot_next(); // edge = edge->rot_next();
// index = ( edge->cell() )->source_index(); // index = ( edge->cell() )->source_index();
// p = discrete_positions[index]/factor; // p = discrete_positions[index]/factor;
// //
// } while( edge != vertex.incident_edge() ); // } while( edge != vertex.incident_edge() );
//*/ //*/
// if(score > max_score) // if(score > max_score)
// { // {
// max_score =score; // max_score =score;
// max_dis = dis; // max_dis = dis;
// max_it = it; // max_it = it;
// } // }
// } // }
...@@ -409,7 +411,7 @@ void VoronoiBestVertexRandMax (const std::vector<Point>& discrete_positions, con ...@@ -409,7 +411,7 @@ void VoronoiBestVertexRandMax (const std::vector<Point>& discrete_positions, con
shuffle(random_numbers.begin(), random_numbers.end(), gen); shuffle(random_numbers.begin(), random_numbers.end(), gen);
double a_random_double = random_numbers[0]; double a_random_double = random_numbers[0];
//the first element in the range [first, last) that is not less than a_random_double //the first element in the range [first, last) that is not less than a_random_double
auto lower = std::lower_bound(partial_sums.begin(), partial_sums.end(), a_random_double); auto lower = std::lower_bound(partial_sums.begin(), partial_sums.end(), a_random_double);
int iposition = lower - partial_sums.begin(); int iposition = lower - partial_sums.begin();
...@@ -505,13 +507,13 @@ void plotVoronoi(const std::vector<Point>& discrete_positions, const voronoi_dia ...@@ -505,13 +507,13 @@ void plotVoronoi(const std::vector<Point>& discrete_positions, const voronoi_dia
{ {
// =============== plot Voronoi Diagram ===================== // =============== plot Voronoi Diagram =====================
char name [50]; char name [50];
sprintf(name, "log_%.3d.py", global_count); sprintf(name, "log_%.3d.py", global_count);
FILE * f; FILE * f;
f = fopen(name, "w"); f = fopen(name, "w");
// plot cells // plot cells
fprintf(f, "# ------------------------------\n"); fprintf(f, "# ------------------------------\n");
fprintf(f, "import matplotlib.pyplot as plt\n"); fprintf(f, "import matplotlib.pyplot as plt\n");
// plot seeds // plot seeds
for(auto pos : discrete_positions) for(auto pos : discrete_positions)
{ {
fprintf(f, "plt.plot([%f], [%f], \"or\")\n", pos._x/factor, pos._y/factor); fprintf(f, "plt.plot([%f], [%f], \"or\")\n", pos._x/factor, pos._y/factor);
...@@ -523,18 +525,18 @@ void plotVoronoi(const std::vector<Point>& discrete_positions, const voronoi_dia ...@@ -523,18 +525,18 @@ void plotVoronoi(const std::vector<Point>& discrete_positions, const voronoi_dia
do { do {
if(edge->vertex0() && edge->vertex1()) if(edge->vertex0() && edge->vertex1())
{ {
fprintf(f, "plt.plot([%f, %f], [%f, %f], \"bo-\", lw=2)\n", edge->vertex0()->x()/factor, edge->vertex1()->x()/factor, edge->vertex0()->y()/factor, edge->vertex1()->y()/factor); fprintf(f, "plt.plot([%f, %f], [%f, %f], \"bo-\", lw=2)\n", edge->vertex0()->x()/factor, edge->vertex1()->x()/factor, edge->vertex0()->y()/factor, edge->vertex1()->y()/factor);
} }
edge = edge->next(); edge = edge->next();
} while (edge != cell.incident_edge()); } while (edge != cell.incident_edge());
} }
// plot geometry // plot geometry
double max_x=std::numeric_limits<double>::min(), min_x=std::numeric_limits<double>::max(); double max_x=std::numeric_limits<double>::min(), min_x=std::numeric_limits<double>::max();
double max_y=std::numeric_limits<double>::min(), min_y=std::numeric_limits<double>::max(); double max_y=std::numeric_limits<double>::min(), min_y=std::numeric_limits<double>::max();
const vector<Point> polygon = subroom->GetPolygon(); const vector<Point> polygon = subroom->GetPolygon();
for(auto it = polygon.begin(); it != polygon.end(); ) { for(auto it = polygon.begin(); it != polygon.end(); ) {
Point gpoint = *(it++); Point gpoint = *(it++);
// Point gpointNext = *it; // Point gpointNext = *it;
if(gpoint._x > max_x) if(gpoint._x > max_x)
max_x = gpoint._x; max_x = gpoint._x;
...@@ -544,7 +546,7 @@ void plotVoronoi(const std::vector<Point>& discrete_positions, const voronoi_dia ...@@ -544,7 +546,7 @@ void plotVoronoi(const std::vector<Point>& discrete_positions, const voronoi_dia
min_x = gpoint._x; min_x = gpoint._x;
if(gpoint._y < min_y) if(gpoint._y < min_y)
min_y = gpoint._y; min_y = gpoint._y;
// fprintf(f, "plt.plot([%f, %f], [%f, %f], \"k-\", lw=2)\n", gpoint._x, gpointNext._x, gpoint._y, gpointNext._y); // fprintf(f, "plt.plot([%f, %f], [%f, %f], \"k-\", lw=2)\n", gpoint._x, gpointNext._x, gpoint._y, gpointNext._y);
if(it == polygon.end()){ if(it == polygon.end()){
// fprintf(f, "plt.plot([%f, %f], [%f, %f], \"k-\", lw=2)\n", gpointNext._x, polygon.begin()->_x, gpointNext._y, polygon.begin()->_y ); // fprintf(f, "plt.plot([%f, %f], [%f, %f], \"k-\", lw=2)\n", gpointNext._x, polygon.begin()->_x, gpointNext._y, polygon.begin()->_y );
......
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