Commit d193aa1e authored by Mohcine Chraibi's avatar Mohcine Chraibi

Make changes to merge voronoi into develop

there are some nasty fixes for peds coming
from a queue who do not have any navline.
This means their v0 can not be calculated, which is
especially bad for 3D geometries (ramps)
parent 1f2e2c84
......@@ -73,7 +73,8 @@ EventManager::EventManager(Building *_b, unsigned int seed)
_file = fopen("../events/events.txt", "r");
if (!_file) {
Log->Write("INFO:\tFiles 'events.txt' missing.");
Log->Write("INFO:\tFiles 'events.txt' missing. "
"Realtime interaction with the simulation not possible.");
} else {
Log->Write("INFO:\tFile 'events.txt' will be monitored for new events.");
_dynamic = true;
......@@ -163,7 +164,9 @@ bool EventManager::ReadEventsXml()
Log->Write("INFO: \tEvents were initialized");
//create some events
CreateSomeEngines();
//FIXME: creating some engine before starting is not working.
// seom doors are still perceived as beeing closed.
//CreateSomeEngines();
return true;
}
......@@ -708,10 +711,15 @@ void EventManager::CreateSomeEngines()
Log->Write("INFO: \tpopulating routers");
std::map<int, bool> doors_states;
for(auto&& t:_building->GetAllTransitions())
{
printf("ID: %d IsOpen: %d\n",t.second->GetID(),t.second->IsOpen());
}
//save the doors states
for(auto&& t:_building->GetAllTransitions())
{
doors_states[t.second->GetID()]= t.second->IsOpen();
doors_states[t.second->GetID()]=t.second->IsOpen();
}
//open all doors
......@@ -747,5 +755,12 @@ void EventManager::CreateSomeEngines()
}
}
Log->Write("INFO: \tdone");
cout<<endl<<endl;
for(auto&& t:_building->GetAllTransitions())
{
printf("ID: %d IsOpen: %d\n",t.second->GetID(),t.second->IsOpen());
}
exit(0);
}
/*
* AgentsSourcesManager.cpp
/**
* \file AgentsSourcesManager.cpp
* \date Apr 14, 2015
* \version v0.7
* \copyright <2009-2015> Forschungszentrum J��lich GmbH. All rights reserved.
*
* Created on: 14.04.2015
* Author: piccolo
*/
* \section License
* This file is part of JuPedSim.
*
* JuPedSim 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 3 of the License, or
* any later version.
*
* JuPedSim 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 General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
*
* \section Description
* This class is responsible for materialising agent in a given location at a given frequency up to a maximum number.
* The optimal position where to put the agents is given by various algorithms, for instance
* the Voronoi algorithm or the Mitchell Best candidate algorithm.
*
**/
#include "AgentsSourcesManager.h"
#include "Pedestrian.h"
#include "StartDistribution.h"
#include "PedDistributor.h"
#include "AgentsSource.h"
#include "../voronoi/VoronoiDiagramGenerator.h"
#include "../geometry/Building.h"
#include "../geometry/Point.h"
#include "../mpi/LCGrid.h"
#include <iostream>
#include <thread>
#include <chrono>
#include "AgentsQueue.h"
#include "../voronoi-boost/VoronoiPositionGenerator.h"
using namespace std;
bool AgentsSourcesManager::_isCompleted=false;
bool AgentsSourcesManager::_isCompleted=true;
AgentsSourcesManager::AgentsSourcesManager()
{
......@@ -42,13 +64,11 @@ void AgentsSourcesManager::Run()
{
Log->Write("INFO:\tStarting agent manager thread");
//Generate all agents required for the complete simulation
//It might be more efficient to generate at each frequency step
for (const auto& src : _sources)
{
src->GenerateAgentsAndAddToPool(src->GetMaxAgents(), _building);
cout<<"generation: "<<src->GetPoolSize()<<endl;
}
//first call ignoring the return value
......@@ -58,52 +78,39 @@ void AgentsSourcesManager::Run()
//it might be better to use a timer
_isCompleted = false;
bool finished = false;
long updateFrequency = 5; // 1 second
long updateFrequency = 2; // 1 = second
do
{
int current_time = Pedestrian::GetGlobalTime();
//first step
//if(current_time==0){
//finished=ProcessAllSources();
// ProcessAllSources();
// //cout<<"here:"<<endl; exit(0);
//}
if ((current_time != _lastUpdateTime)
&& ((current_time % updateFrequency) == 0))
{
//cout<<"TIME:"<<current_time<<endl;
finished=ProcessAllSources();
_lastUpdateTime = current_time;
cout << "source size: " << _sources.size() << endl;
}
//wait some time
//cout<<"sleepinp..."<<endl;
//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;//exit(0);
_isCompleted = true;
}
bool AgentsSourcesManager::ProcessAllSources() const
{
bool empty=true;
//cout<<"src size: "<<_sources.size()<<endl;
for (const auto& src : _sources)
{
//cout<<"size: "<<src->GetPoolSize()<<endl;//exit(0);
if (src->GetPoolSize())
{
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);
//ComputeBestPositionRandom(src.get(), peds);
//todo: compute the optimal position for insertion using voronoi
//for (auto&& ped : peds)
//{
//ComputeBestPositionVoronoi(src.get(), ped);
//ped->Dump(ped->GetID());
//}
if( !ComputeBestPositionVoronoiBoost(src.get(), peds, _building) )
Log->Write("INFO:\t there was no place for some pedestrians");
AgentsQueueIn::Add(peds);
empty = false;
}
......@@ -112,6 +119,58 @@ 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.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[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);
}
}
void AgentsSourcesManager::ComputeBestPositionCompleteRandom(AgentsSource* src,
vector<Pedestrian*>& peds)const
{
auto dist = src->GetStartDistribution();
auto subroom = _building->GetRoom(dist->GetRoomId())->GetSubRoom(dist->GetSubroomID());
vector<Point> positions = PedDistributor::PossiblePositions(*subroom);
//TODO: get the seed from the simulation
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;
}
}
}
/*
void AgentsSourcesManager::ComputeBestPositionVoronoi(AgentsSource* src,
Pedestrian* agent) const
{
......@@ -133,7 +192,7 @@ void AgentsSourcesManager::ComputeBestPositionVoronoi(AgentsSource* src,
&& (bounds[1] <= pos._y && pos._y <= bounds[2]))
{
iter = peds.erase(iter);
cout << "removing..." << endl;
cout << "removing (testing only)..." << endl;
exit(0);
} else
{
......@@ -186,10 +245,10 @@ void AgentsSourcesManager::ComputeBestPositionVoronoi(AgentsSource* src,
for (auto&& ped : peds)
{
double distance = (pos - ped->GetPos()).NormSquare();
if (distance < min_dist)
double dist = (pos - ped->GetPos()).NormSquare();
if (dist < min_dist)
{
min_dist = distance;
min_dist = dist;
}
}
map_dist_to_position[min_dist] = pos;
......@@ -228,6 +287,9 @@ void AgentsSourcesManager::ComputeBestPositionVoronoi(AgentsSource* src,
//compute the best position
//exit(0);
}
*/
void AgentsSourcesManager::ComputeBestPositionRandom(AgentsSource* src,
std::vector<Pedestrian*>& peds) const
......@@ -241,25 +303,50 @@ void AgentsSourcesManager::ComputeBestPositionRandom(AgentsSource* src,
double bounds[4] = { 0, 0, 0, 0 };
dist->Getbounds(bounds);
std::vector<Pedestrian*> peds_without_place;
vector<Point> extra_positions;
for (auto& ped : peds)
std::vector<Pedestrian*>::iterator iter_ped;
for (iter_ped = peds.begin(); iter_ped != peds.end(); )
{
//need to be called at each iteration
SortPositionByDensity(positions, extra_positions);
int index = -1;
double radius = ( (*iter_ped)->GetEllipse() ).GetBmax() ;
//in the case a range was specified
//just take the first element
for (unsigned int a = 0; a < positions.size(); a++)
{
Point pos = positions[a];
//cout<<"checking: "<<pos.toString()<<endl;
// for positions inside bounds, check it there is enough space
if ((bounds[0] <= pos._x) && (pos._x <= bounds[1])
&& (bounds[2] <= pos._y) && (pos._y < bounds[3]))
{
index = a;
break;
bool enough_space = true;
//checking enough space!!
vector<Pedestrian*> neighbours;
_building->GetGrid()->GetNeighbourhood(pos,neighbours);
for (const auto& ngh: neighbours)
if( (ngh->GetPos() - pos).NormSquare() < 4*radius*radius )
{
enough_space = false;
break;
}
if( enough_space )
{
index = a;
break;
}
}
}
if (index == -1)
......@@ -269,22 +356,35 @@ void AgentsSourcesManager::ComputeBestPositionRandom(AgentsSource* src,
Log->Write(
"ERROR:\t AgentSourceManager 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");
Log->Write(" \t Specifying a subroom_id might help");
Log->Write(" \t %d positions were available",positions.size());
//exit(EXIT_FAILURE);
}
//dump the pedestrian, move iterator
peds_without_place.push_back(*iter_ped);
iter_ped=peds.erase(iter_ped);
}
else
else //we found a position with enough space
{
const Point& pos = positions[index];
extra_positions.push_back(pos);
ped->SetPos(pos, true); //true for the initial position
positions.erase(positions.begin() + index);
//at this point we have a position
//so we can adjust the velocity
//AdjustVelocityUsingWeidmann(ped);
AdjustVelocityByNeighbour(ped);
extra_positions.push_back(pos);
(*iter_ped)->SetPos(pos, true); //true for the initial position
positions.erase(positions.begin() + index);
//at this point we have a position
//so we can adjust the velocity
//AdjustVelocityUsingWeidmann(ped);
AdjustVelocityByNeighbour( (*iter_ped) );
//move iterator
iter_ped++;
}
//return the pedestrians without place
}
if(peds_without_place.size()>0)
src->AddAgentsToPool(peds_without_place);
}
void AgentsSourcesManager::AdjustVelocityByNeighbour(Pedestrian* ped) const
......@@ -320,8 +420,8 @@ void AgentsSourcesManager::AdjustVelocityByNeighbour(Pedestrian* ped) const
//mean speed
if(count==0)
{
speed=ped->GetEllipse().GetV0(); // FIXME: bad fix for: peds without navline (ar.graf)
//speed=ped->GetV0Norm();
speed=ped->GetEllipse().GetV0(); //bad fix for: peds without navline (ar.graf)
}
else
{
......@@ -402,12 +502,12 @@ void AgentsSourcesManager::SortPositionByDensity(std::vector<Point>& positions,
_building->GetGrid()->GetNeighbourhood(pt,neighbours);
//density in pers per m2
double density = 0.0;
//double radius_square=0.56*0.56;
double radius_square=0.40*0.40;
for(const auto& p: neighbours)
{
if( (pt-p->GetPos()).NormSquare()<=radius_square)
//FIXME: p can be null, if deleted in the main simulation thread.
if( p && (pt-p->GetPos()).NormSquare()<=radius_square)
density+=1.0;
}
......@@ -446,6 +546,7 @@ void AgentsSourcesManager::GenerateAgents()
void AgentsSourcesManager::AddSource(std::shared_ptr<AgentsSource> src)
{
_sources.push_back(src);
_isCompleted=false;//at least one source was provided
}
const std::vector<std::shared_ptr<AgentsSource> >& AgentsSourcesManager::GetSources() const
......
......@@ -523,6 +523,11 @@ double Pedestrian::GetV0Norm() const
//detect the walking direction based on the elevation
SubRoom* sub=_building->GetRoom(_roomID)->GetSubRoom(_subRoomID);
double ped_elevation = sub->GetElevation(_ellipse.GetCenter());
if (_navLine ==nullptr)
{
printf("Error: ped %d has no navline\n", _id);
exit(EXIT_FAILURE);
}
const Point& target = _navLine->GetCentre();
double nav_elevation = sub->GetElevation(target);
double delta = nav_elevation - ped_elevation;
......
......@@ -79,197 +79,198 @@ bool IsEnoughInSubroom( SubRoom* subroom, Point& pt, double radius )
return true;
}
//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();
//
// std::vector<Pedestrian*> existing_peds;
// std::vector<Pedestrian*> peds_without_place;
// building->GetPedestrians(roomID, subroomID, existing_peds);
//
// double radius = 0.3; //radius of a person, 0.3 is just some number(needed for the fake_peds bellow), will be changed afterwards
//
// SubRoom* subroom = building->GetRoom( roomID )->GetSubRoom(subroomID);
//
// double factor = 100; //factor for conversion to integer for the boost voronoi
//
// std::vector<Point> fake_peds;
// Point temp(0,0);
// //fake_peds will be the positions of "fake" pedestrians, multiplied by factor and converted to int
// for (auto vert: subroom->GetPolygon() ) //room vertices
// {
// const Point& center_pos = subroom->GetCentroid();
// temp._x = ( center_pos._x-vert._x );
// temp._y = ( center_pos._y-vert._y );
// 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 + vert;
// temp._x = (int)(temp._x*factor);
// temp._y = (int)(temp._y*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);
// radius = ped->GetEllipse().GetBmax(); //max radius of the current pedestrian
//
// if(existing_peds.size() == 0 )
// {
// const Point& center_pos = subroom->GetCentroid();
//
// 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;
// //this could be better, but needs to work with any polygon - random point inside a polygon?
// if ( subroom->IsInSubRoom( new_pos ) )
// {
// if( IsEnoughInSubroom(subroom, new_pos, radius ) )
// {
// ped->SetPos(center_pos + random_pos, true);
// Point v;
// if (ped->GetExitLine()) {
// v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
// } else {
// v = Point(0., 0.);
// }
// double speed=ped->GetV0Norm();
// v=v*speed;
// ped->SetV(v);
// }
// else
// {
// ped->SetPos(center_pos, true);
// Point v;
// if (ped->GetExitLine()) {
// v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
// } else {
// v = Point(0., 0.);
// }
// double speed=ped->GetV0Norm();
// v=v*speed;
// ped->SetV(v);
// }
// }
// else
// {
// ped->SetPos(center_pos, true);
// Point v;
// if (ped->GetExitLine()) {
// v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
// } else {
// v = Point(0., 0.);
// }
// double speed=ped->GetV0Norm();
// v=v*speed;
// ped->SetV(v);
// }
//
// existing_peds.push_back(ped);
//
//
// }//0
//
// else //more than one pedestrian
// {
// //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
// std::vector<Point> discrete_positions;
// std::vector<Point> velocities_vector;
// std::vector<int> goal_vector;
// Point tmp(0,0);
// Point v(0,0);
// double no = 0;
//
// //points from double to integer
// for (const auto& eped: existing_peds)
// {
// const Point& pos = eped->GetPos();
// tmp._x = (int)( pos._x*factor );
// tmp._y = (int)( pos._y*factor );
// discrete_positions.push_back( tmp );
// velocities_vector.push_back( eped->GetV() );
// goal_vector.push_back( eped->GetFinalDestination() );
//
// //calculating the mean, using it for the fake pedestrians
// v = v + eped->GetV();
// no++;
// }
//
// //TODO: dividing by 0 when existing_peds is empty
// // -------> existing_peds is not empty because of the if statement
// // sum up the weighted velocity in the loop
// v = v/no; //this is the mean of all velocities
//
// //adding fake people to the vector for constructing voronoi diagram
// for (unsigned int i=0; i<subroom->GetPolygon().size(); i++ )
// {
// discrete_positions.push_back( fake_peds[i] );
// velocities_vector.push_back( v );
// goal_vector.push_back( -10 );
// }
//
// //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, radius );
//
// if( dis > 4*radius*factor*radius*factor)// be careful with the factor!! radius*factor, 2,3,4?
// {
// Point pos( chosen_it->x()/factor, chosen_it->y()/factor ); //check!
// ped->SetPos(pos , true);
// VoronoiAdjustVelocityNeighbour(chosen_it, ped, velocities_vector, goal_vector);
//
// // proceed to the next pedestrian
// existing_peds.push_back(ped);
// ++iter_ped;
//
// }
// else
// {
// //reject the pedestrian:
// return_value = false;
// peds_without_place.push_back(*iter_ped); //Put in a different queue, they will be put back in the source.
// iter_ped=peds.erase(iter_ped); // remove from the initial vector since it should only contain the pedestrians that could find a place
// }
//
// /*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
// {
// 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
// }
// }*/
// }// >0
//
//
// }//for loop
//
//
// //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;
//}
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();
std::vector<Pedestrian*> existing_peds;
std::vector<Pedestrian*> peds_without_place;
building->GetPedestrians(roomID, subroomID, existing_peds);
double radius = 0.3; //radius of a person, 0.3 is just some number(needed for the fake_peds bellow), will be changed afterwards
SubRoom* subroom = building->GetRoom( roomID )->GetSubRoom(subroomID);
double factor = 100; //factor for conversion to integer for the boost voronoi
std::vector<Point> fake_peds;
Point temp(0,0);
//fake_peds will be the positions of "fake" pedestrians, multiplied by factor and converted to int
for (auto vert: subroom->GetPolygon() ) //room vertices
{
const Point& center_pos = subroom->GetCentroid();
temp._x = ( center_pos._x-vert._x );
temp._y = ( center_pos._y-vert._y );
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 + vert;
temp._x = (int)(temp._x*factor);
temp._y = (int)(temp._y*factor);
fake_peds.push_back( temp );
}