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
}
void Simulation::UpdateRoutesAndLocations()
{
//pedestrians to be deleted
//you should better create this in the constructor and allocate it once.
set<Pedestrian*> pedsToRemove;
// pedsToRemove.reserve(500); //just reserve some space
// collect all pedestrians in the simulation.
const vector<Pedestrian*>& allPeds = _building->GetAllPedestrians();
const map<int, Goal*>& goals = _building->GetAllGoals();
auto allRooms = _building->GetAllRooms();
#pragma omp parallel for shared(pedsToRemove, allRooms)
for (signed int p = 0; p < allPeds.size(); ++p) {
auto ped = allPeds[p];
Room* room = _building->GetRoom(ped->GetRoomID());
SubRoom* sub0 = room->GetSubRoom(ped->GetSubRoomID());
//set the new room if needed
if ((ped->GetFinalDestination() == FINAL_DEST_OUT)
&& (room->GetCaption() == "outside")) {
#pragma omp critical(Simulation_Update_pedsToRemove)
pedsToRemove.insert(ped);
} else if ((ped->GetFinalDestination() != FINAL_DEST_OUT)
&& (goals.at(ped->GetFinalDestination())->Contains(
ped->GetPos()))) {
#pragma omp critical(Simulation_Update_pedsToRemove)
pedsToRemove.insert(ped);
}
// reposition in the case the pedestrians "accidently left the room" not via the intended exit.
// That may happen if the forces are too high for instance
// the ped is removed from the simulation, if it could not be reassigned
else if (!sub0->IsInSubRoom(ped))
{
bool assigned = false;
std::function<void(const Pedestrian&)> f = std::bind(&Simulation::UpdateFlowAtDoors, this, std::placeholders::_1);
assigned = ped->Relocate(f);
//this will delete agents, that are pushed outside (maybe even if inside obstacles??)
if (!assigned) {
#pragma omp critical(Simulation_Update_pedsToRemove)
pedsToRemove.insert(ped); //the agent left the old room
//actualize the eggress time for that room
#pragma omp critical(SetEgressTime)
allRooms.at(ped->GetRoomID())->SetEgressTime(ped->GetGlobalTime());
}
}
//finally actualize the route
if ( !_gotSources && ped->FindRoute() == -1) {
//a destination could not be found for that pedestrian
Log->Write("ERROR: \tCould not find a route for pedestrian %d",ped->GetID());
//ped->FindRoute(); //debug only, plz remove
std::function<void(const Pedestrian&)> f = std::bind(&Simulation::UpdateFlowAtDoors, this, std::placeholders::_1);
ped->Relocate(f);
//exit(EXIT_FAILURE);
#pragma omp critical(Simulation_Update_pedsToRemove)
{
pedsToRemove.insert(ped);
Log->incrementDeletedAgents();
}
}
}
#ifdef _USE_PROTOCOL_BUFFER
if (_hybridSimManager)
{
AgentsQueueOut::Add(pedsToRemove); //this should be critical region (and it is)
}
else
#endif
{
// remove the pedestrians that have left the building
for (auto p : pedsToRemove){
UpdateFlowAtDoors(*p);
_building->DeletePedestrian(p);
}
pedsToRemove.clear();
}
// temporary fix for the safest path router
// if (dynamic_cast<SafestPathRouter*>(_routingEngine->GetRouter(1)))
// {
// SafestPathRouter* spr = dynamic_cast<SafestPathRouter*>(_routingEngine->GetRouter(1));
// spr->ComputeAndUpdateDestinations(_allPedestians);
// }
}