void VelocityModel::ComputeNextTimeStep(double current, double deltaT, Building* building, int periodic)
{
double delta = 0.5;
// collect all pedestrians in the simulation.
const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();
unsigned long nSize;
nSize = allPeds.size();
int nThreads = omp_get_max_threads();
//nThreads = 1; //debug only
int partSize;
partSize = (int) (nSize / nThreads);
#pragma omp parallel default(shared) num_threads(nThreads)
{
vector< Point > result_acc = vector<Point > ();
result_acc.reserve(nSize);
vector< my_pair > spacings = vector<my_pair > ();
spacings.reserve(nSize); // larger than needed
spacings.push_back(my_pair(100, 1)); // in case there are no neighbors
const int threadID = omp_get_thread_num();
int start = threadID*partSize;
int end;
end = (threadID + 1) * partSize - 1;
if ((threadID == nThreads - 1)) end = (int) (nSize - 1);
for (int p = start; p <= end; ++p) {
Pedestrian* ped = allPeds[p];
Room* room = building->GetRoom(ped->GetRoomID());
SubRoom* subroom = room->GetSubRoom(ped->GetSubRoomID());
double normVi = ped->GetV().ScalarProduct(ped->GetV()); //squared
double HighVel = (ped->GetV0Norm() + delta) * (ped->GetV0Norm() + delta); //(v0+delta)^2
if (normVi > HighVel && ped->GetV0Norm() > 0) {
fprintf(stderr, "WARNING: VelocityModel::ComputeNextTimeStep() actual velocity (%f) of iped %d "
"is bigger than desired velocity (%f) at time: %fs\n",
sqrt(normVi), ped->GetID(), ped->GetV0Norm(), current);
// remove the pedestrian and abort
Log->Write("\tERROR: ped [%d] was removed due to high velocity",ped->GetID());
building->DeletePedestrian(ped);
exit(EXIT_FAILURE);
}
Point repPed = Point(0,0);
vector<Pedestrian*> neighbours;
building->GetGrid()->GetNeighbourhood(ped,neighbours);
int size = (int) neighbours.size();
for (int i = 0; i < size; i++) {
Pedestrian* ped1 = neighbours[i];
//if they are in the same subroom
Point p1 = ped->GetPos();
Point p2 = ped1->GetPos();
//subrooms to consider when looking for neighbour for the 3d visibility
vector<SubRoom*> emptyVector;
emptyVector.push_back(subroom);
emptyVector.push_back(building->GetRoom(ped1->GetRoomID())->GetSubRoom(ped1->GetSubRoomID()));
bool isVisible = building->IsVisible(p1, p2, emptyVector, false);
if (!isVisible)
continue;
if (ped->GetUniqueRoomID() == ped1->GetUniqueRoomID()) {
repPed += ForceRepPed(ped, ped1, periodic);
} else {
// or in neighbour subrooms
SubRoom* sb2=building->GetRoom(ped1->GetRoomID())->GetSubRoom(ped1->GetSubRoomID());
if(subroom->IsDirectlyConnectedWith(sb2)) {
repPed += ForceRepPed(ped, ped1, periodic);
}
}
} // for i
//repulsive forces to walls and closed transitions that are not my target
Point repWall = ForceRepRoom(allPeds[p], subroom);
// calculate new direction ei according to (6)
Point direction = e0(ped, room) + repPed + repWall;
for (int i = 0; i < size; i++) {
Pedestrian* ped1 = neighbours[i];
// calculate spacing
// my_pair spacing_winkel = GetSpacing(ped, ped1);
if (ped->GetUniqueRoomID() == ped1->GetUniqueRoomID()) {
spacings.push_back(GetSpacing(ped, ped1, direction, periodic));
} else {
// or in neighbour subrooms
SubRoom* sb2=building->GetRoom(ped1->GetRoomID())->GetSubRoom(ped1->GetSubRoomID());
if(subroom->IsDirectlyConnectedWith(sb2)) {
spacings.push_back(GetSpacing(ped, ped1, direction, periodic));
}
}
}
// @todo: get spacing to walls
// @todo: update direction every DT?
// if(ped->GetID()==-10)
// std::cout << "time: " << ped->GetGlobalTime() << " | updateRate " <<ped->GetUpdateRate() << " modulo " <<fmod(ped->GetGlobalTime(), ped->GetUpdateRate())<<std::endl;
// calculate min spacing
std::sort(spacings.begin(), spacings.end(), sort_pred());
double spacing = spacings[0].first;
double winkel = spacings[0].second;
Point tmp;
// if(fmod(ped->GetGlobalTime(), ped->GetUpdateRate())<0.0001 || (spacing-ped->GetLastE0()._x)>0.01)
// {
// if(ped->GetID()==-10)
// std::cout << "Min Spacing "<< spacing << ", " << winkel << std::endl;
// ped->SetLastE0(Point(spacing, winkel));
// }
// else
// {
// tmp = ped->GetLastE0();
// if(ped->GetID()==10)
// std::cout << "keep direction "<<tmp._x << ", " << tmp._y << std::endl;
// spacing = tmp._x;
// winkel = tmp._y;
// }
// if(ped->GetID()==-10)
// getc(stdin);
// spacing = *std::min_element(std::begin(spacings), std::end(spacings));
// if(ped->GetID()==10){
// fprintf(stderr, "%f %f %f\n", ped->GetGlobalTime(), (repPed+repWall).Norm(), spacing);
// }
Point speed = direction.Normalized() * OptimalSpeed(ped, spacing, winkel);
result_acc.push_back(speed);
spacings.clear(); //clear for ped p
} // for p
//#pragma omp barrier
// update
for (int p = start; p <= end; ++p) {
Pedestrian* ped = allPeds[p];
Point v_neu = result_acc[p - start];
Point pos_neu = ped->GetPos() + v_neu * deltaT;
//Jam is based on the current velocity
if ( v_neu.Norm() >= ped->GetV0Norm()*0.5) {
ped->ResetTimeInJam();
} else {
ped->UpdateTimeInJam();
}
//only update the position if the velocity is above a threshold
if (v_neu.Norm() >= J_EPS_V)
{
ped->SetPhiPed();
}
ped->SetPos(pos_neu);
if(periodic){
if(ped->GetPos()._x >= xRight){
ped->SetPos(Point(ped->GetPos()._x - (xRight - xLeft), ped->GetPos()._y));
//ped->SetID( ped->GetID() + 1);
}
}
ped->SetV(v_neu);
}
}//end parallel
}
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;
}