void TrajectoriesJPSV05::WriteFrame(int frameNr, Building* building)
{
string data;
char tmp[CLENGTH] = "";
double RAD2DEG = 180.0 / M_PI;
sprintf(tmp, "<frame ID=\"%d\">\n", frameNr);
data.append(tmp);
const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();
for(unsigned int p=0;p<allPeds.size();p++)
{
Pedestrian* ped = allPeds[p];
Room* r = building->GetRoom(ped->GetRoomID());
string caption = r->GetCaption();
char s[CLENGTH] = "";
int color=ped->GetColor();
double a = ped->GetLargerAxis();
double b = ped->GetSmallerAxis();
double phi = atan2(ped->GetEllipse().GetSinPhi(), ped->GetEllipse().GetCosPhi());
sprintf(s, "<agent ID=\"%d\"\t"
"x=\"%.6f\"\ty=\"%.6f\"\t"
"z=\"%.6f\"\t"
"rA=\"%.2f\"\trB=\"%.2f\"\t"
"eO=\"%.2f\" eC=\"%d\"/>\n",
ped->GetID(), (ped->GetPos()._x) * FAKTOR,
(ped->GetPos()._y) * FAKTOR,(ped->GetElevation()) * FAKTOR ,a * FAKTOR, b * FAKTOR,
phi * RAD2DEG, color);
data.append(s);
}
data.append("</frame>\n");
_outputHandler->Write(data);
}
void TrajectoriesFLAT::WriteFrame(int frameNr, Building* building)
{
char tmp[CLENGTH] = "";
const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();
for(unsigned int p=0;p<allPeds.size();p++){
Pedestrian* ped = allPeds[p];
double x = ped->GetPos()._x;
double y = ped->GetPos()._y;
double z = ped->GetElevation();
sprintf(tmp, "%d\t%d\t%0.2f\t%0.2f\t%0.2f", ped->GetID(), frameNr, x, y,z);
Write(tmp);
}
}
void TrajectoriesJPSV06::WriteFrame(int frameNr, Building* building)
{
string data;
char tmp[CLENGTH] = "";
double RAD2DEG = 180.0 / M_PI;
vector<string> rooms_to_plot;
sprintf(tmp, "<frame ID=\"%d\">\n", frameNr);
data.append(tmp);
const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();
for(unsigned int p=0;p<allPeds.size();++p)
{
Pedestrian* ped = allPeds[p];
Room* r = building->GetRoom(ped->GetRoomID());
string caption = r->GetCaption();
if (!IsElementInVector(rooms_to_plot, caption)) {
if (!rooms_to_plot.empty()) {
continue;
}
}
char tmp1[CLENGTH] = "";
int color=ped->GetColor();
double a = ped->GetLargerAxis();
double b = ped->GetSmallerAxis();
double phi = atan2(ped->GetEllipse().GetSinPhi(), ped->GetEllipse().GetCosPhi());
sprintf(tmp1, "<agent ID=\"%d\"\t"
"x=\"%.6f\"\ty=\"%.6f\"\t"
"z=\"%.6f\"\t"
"rA=\"%.2f\"\trB=\"%.2f\"\t"
"eO=\"%.2f\" eC=\"%d\"/>\n",
ped->GetID(), (ped->GetPos()._x) * FAKTOR,
(ped->GetPos()._y) * FAKTOR,(ped->GetElevation()) * FAKTOR ,a * FAKTOR, b * FAKTOR,
phi * RAD2DEG, color);
data.append(tmp1);
}
data.append("</frame>\n");
Write(data);
}
void Simulation::UpdateFlowAtDoors(const Pedestrian& ped) const
{
if (_config->ShowStatistics()) {
Transition* trans = _building->GetTransitionByUID(ped.GetExitIndex());
if (trans) {
//check if the pedestrian left the door correctly
if (trans->DistTo(ped.GetPos())>0.5) {
Log->Write("WARNING:\t pedestrian [%d] left room/subroom [%d/%d] in an unusual way. Please check",
ped.GetID(), ped.GetRoomID(), ped.GetSubRoomID());
Log->Write(" :\t distance to last door (%d | %d) is %f. That should be smaller.",
trans->GetUniqueID(), ped.GetExitIndex(),
trans->DistTo(ped.GetPos()));
Log->Write(" :\t correcting the door statistics");
//ped.Dump(ped.GetID());
//checking the history and picking the nearest previous destination
double biggest = 0.3;
bool success = false;
for (const auto& dest:ped.GetLastDestinations()) {
if (dest!=-1) {
Transition* trans_tmp = _building->GetTransitionByUID(dest);
if (trans_tmp && trans_tmp->DistTo(ped.GetPos())<biggest) {
biggest = trans_tmp->DistTo(ped.GetPos());
trans = trans_tmp;
Log->Write(" :\t Best match found at door %d", dest);
success = true;//at least one door was found
}
}
}
if (!success) {
Log->Write("WARNING :\t correcting the door statistics");
return; //todo we need to check if the ped is in a subroom neighboring the target. If so, no problems!
}
}
//#pragma omp critical
trans->IncreaseDoorUsage(1, ped.GetGlobalTime());
}
}
}
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;
}
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
}