void TrajectoriesJPSV04::WriteFrame(int frameNr, Building* building)
{
string data;
char tmp[CLENGTH] = "";
vector<string> rooms_to_plot;
if( building->GetAllPedestrians().size() == 0)
return;
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 ((rooms_to_plot.empty() == false)
&& (IsElementInVector(rooms_to_plot, caption) == false)) {
continue;
}
data.append(WritePed(ped));
}
data.append("</frame>\n");
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);
}
}
int main()
{
Entity * e = new Entity("1", 1);
Entity * wrld = new Entity("0", 0);
e->m_location.m_loc = wrld;
e->m_location.m_loc->makeContainer();
assert(e->m_location.m_loc->m_contains != 0);
e->m_location.m_loc->m_contains->insert(e);
TestWorld test_world(*wrld);
Pedestrian * p = new Pedestrian(*e);
p->getTickAddition(Point3D(0,0,0), Vector3D(1,0,0));
p->setTarget(Point3D(2,0,0));
p->getTickAddition(Point3D(0,0,0), Vector3D(1,0,0));
Location loc;
p->getUpdatedLocation(loc);
e->m_location.m_velocity = Vector3D(1,0,0);
p->getUpdatedLocation(loc);
e->m_location.m_pos = Point3D(1,0,0);
p->getUpdatedLocation(loc);
e->m_location.m_pos = Point3D(2,0,0);
p->getUpdatedLocation(loc);
e->m_location.m_pos = Point3D(3,0,0);
p->getUpdatedLocation(loc);
p->generateMove(loc);
delete p;
return 0;
}
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
}
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;
}
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());
}
}
}
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 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);
}
PotentialPlanner2::Vector2D PotentialPlanner2::calcForce(Pedestrian &apedestrian)
{
State astate = apedestrian.getState();
bool danger=false;
bool veryDangerous=false;
bool semiDangerous=false;
Vector2D resForce;
dd dx = car->getX() - astate.x;
dd dy = car->getY() - astate.y;
if (dy > Y_MAX -Y_VISIBLE) dy -= (Y_MAX - Y_MIN);
dd dist = sqrt(dx*dx + dy*dy);
dd y_factor = DANGEROUS_Y_DIST/dist;
dd x_factor = DANGEROUS_X_DIST/(abs(dx)+0.01);
dd v_factor = car->getV()/DANGEROUS_VELOCITY;
//Find how dangerous is the pedestrian situation
if (isVeryDangerous(astate))
{
car->setExistVeryDangerous(true);
veryDangerous = true;
apedestrian.setColor(4);
}
else if (isDangerous(astate))
{
danger = true;
apedestrian.setColor(1);
}
else if (isSemiDangerous(astate))
{
semiDangerous = true;
apedestrian.setColor(3);
}
else
{
apedestrian.setColor(0);
}
//Calculating the general force (not dangerous)
dd forceVal = m_charge*car->getV()/(dist*dist);
if (dy > -car->getLength()/2) forceVal=0.0;
resForce.x = forceVal*dx/dist;
resForce.y = (forceVal*dy/dist)*v_factor*v_factor;
//Adjusting the force to the dangerous level
if (danger && !veryDangerous)
{
assert (dy <= hlength+BUFFER_DIST);
if (fabs(atan2(-dx,-dy)) < M_PI/7.0 )
resForce.x = -6.0*cos(astate.theta)*fabs(resForce.x*x_factor*x_factor);
else
resForce.x *= 6.0;
//bool tempAssert = (resForce.x*apedestrian.getXDot() <= 0.001);
resForce.y *= 1.5e3*y_factor*y_factor*v_factor*v_factor*v_factor;
if (dist < 8.0) resForce.y *= x_factor*x_factor/4;
}
else if (veryDangerous)
{
double theta = car->getTheta();
double bigCharge = 1e10;
resForce.x -= bigCharge*cos(theta);
resForce.y -= bigCharge*sin(theta);
}
else if (semiDangerous)
{
resForce.x *= 6.0;
resForce.y *= 6.0;
}
if (resForce.y > 0) resForce.y = 0;
return resForce;
}
