bool VelocityModel::Init (Building* building)
{
if(dynamic_cast<DirectionFloorfield*>(_direction)){
Log->Write("INFO:\t Init DirectionFloorfield starting ...");
//fix using defaults; @fixme ar.graf (pass params from argument parser to ctor?)
double _deltaH = 0.0625;
double _wallAvoidDistance = 0.4;
bool _useWallAvoidance = true;
dynamic_cast<DirectionFloorfield*>(_direction)->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
Log->Write("INFO:\t Init DirectionFloorfield done");
}
if(dynamic_cast<DirectionLocalFloorfield*>(_direction)){
Log->Write("INFO:\t Init DirectionLOCALFloorfield starting ...");
//fix using defaults; @fixme ar.graf (pass params from argument parser to ctor?)
double _deltaH = 0.0625;
double _wallAvoidDistance = 0.4;
bool _useWallAvoidance = true;
dynamic_cast<DirectionLocalFloorfield*>(_direction)->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
Log->Write("INFO:\t Init DirectionLOCALFloorfield done");
}
if(dynamic_cast<DirectionSubLocalFloorfield*>(_direction)){
Log->Write("INFO:\t Init DirectionSubLOCALFloorfield starting ...");
//fix using defaults; @fixme ar.graf (pass params from argument parser to ctor?)
double _deltaH = 0.0625;
double _wallAvoidDistance = 0.4;
bool _useWallAvoidance = true;
dynamic_cast<DirectionSubLocalFloorfield*>(_direction)->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
Log->Write("INFO:\t Init DirectionSubLOCALFloorfield done");
}
const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();
size_t peds_size = allPeds.size();
for(unsigned int p=0;p < peds_size;p++)
{
Pedestrian* ped = allPeds[p];
double cosPhi, sinPhi;
//a destination could not be found for that pedestrian
if (ped->FindRoute() == -1) {
Log->Write(
"ERROR:\tVelocityModel::Init() cannot initialise route. ped %d is deleted.\n",ped->GetID());
building->DeletePedestrian(ped);
p--;
peds_size--;
continue;
}
Point target = ped->GetExitLine()->LotPoint(ped->GetPos());
Point d = target - ped->GetPos();
double dist = d.Norm();
if (dist != 0.0) {
cosPhi = d._x / dist;
sinPhi = d._y / dist;
} else {
Log->Write(
"ERROR: \tallPeds::Init() cannot initialise phi! "
"dist to target is 0\n");
return false;
}
ped->InitV0(target);
JEllipse E = ped->GetEllipse();
E.SetCosPhi(cosPhi);
E.SetSinPhi(sinPhi);
ped->SetEllipse(E);
}
return true;
}
bool ComputeBestPositionVoronoiBoost(AgentsSource* src, std::vector<Pedestrian*>& peds,
Building* building, std::vector<Pedestrian*>& peds_queue)
{
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);
existing_peds.insert(existing_peds.end(), peds_queue.begin(), peds_queue.end());
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/temp.Norm();
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;
std::srand(0);
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);
}
}
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();
double speed = ped->GetEllipse().GetV0(); //@todo: some peds do not have a navline. This should not be accepted.
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 += eped->GetV();
no++;
}
// 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++ )
for(auto fake_ped: fake_peds)
{
discrete_positions.push_back( fake_ped );
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);
#if PLOT_VORONOI_DIAGRAM
plotVoronoi(discrete_positions, vd, subroom, factor);
#endif
voronoi_diagram<double>::const_vertex_iterator chosen_it = vd.vertices().begin();
double dis = 0;
//std::default_random_engine gen = dist->GetGenerator();
if(!src->Greedy())
VoronoiBestVertexRandMax(discrete_positions, vd, subroom, factor, chosen_it, dis, radius);
else
VoronoiBestVertexGreedy(discrete_positions, vd, subroom, factor, chosen_it, dis, radius);
if( dis > 4*radius*radius)
{
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
}
}// >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;
}