diff --git a/Simulation.cpp b/Simulation.cpp index bf228b774a9263009b18ad061ab60596e4e974f0..87508c782d0ea1986823bf4ad516e416db4a7bfc 100644 --- a/Simulation.cpp +++ b/Simulation.cpp @@ -256,7 +256,10 @@ bool Simulation::InitArgs() Log->Write("INFO\tMax : %d",TT.second->nmax); Log->Write("INFO\tnumber doors : %d\n",TT.second->doors.size()); } - Log->Write("Got %d Train Time Tables",_building->GetTrainTimeTables().size()); + if(_building->GetTrainTimeTables().size()) + Log->Write("INFO:\tGot %d Train Time Tables",_building->GetTrainTimeTables().size()); + else + Log->Write("WARNING:\tGot %d Train Time Tables",_building->GetTrainTimeTables().size()); for(auto&& TT: _building->GetTrainTimeTables()) { Log->Write("INFO\tid : %d",TT.second->id); @@ -597,10 +600,11 @@ double Simulation::RunBody(double maxSimTime) } else{ // quickest needs update even if NeedsUpdate() is false FFRouter* ffrouter = dynamic_cast(_routingEngine.get()->GetRouter(ROUTING_FF_QUICKEST)); - if (ffrouter->MustReInit()) { - ffrouter->ReInit(); - ffrouter->SetRecalc(t); - } + if(ffrouter != nullptr) + if (ffrouter->MustReInit()) { + ffrouter->ReInit(); + ffrouter->SetRecalc(t); + } } // here the used routers are update, when needed due to external changes @@ -846,15 +850,29 @@ bool Simulation::correctGeometry(std::shared_ptr building, std::shared NewWall1.SetType(w1.GetType()); // add new lines to be controled against overlap with exits - building->TempAddedWalls[trainId].push_back(NewWall); - building->TempAddedWalls[trainId].push_back(NewWall1); + if(NewWall.GetLength() > J_EPS_DIST) + { + building->TempAddedWalls[trainId].push_back(NewWall); + subroom->AddWall(NewWall); + } + + else + std::cout << KRED << ">> WALL did not add: " << NewWall.toString() << "\n" << RESET ; + + if(NewWall1.GetLength() > J_EPS_DIST) + { + building->TempAddedWalls[trainId].push_back(NewWall1); + subroom->AddWall(NewWall1); + } + else + std::cout << KRED << ">> WALL did not add: " << NewWall1.toString() << "\n" << RESET ; + building->TempAddedDoors[trainId].push_back(*e); building->TempRemovedWalls[trainId].push_back(w1); - subroom->AddWall(NewWall); - subroom->AddWall(NewWall1); + subroom->RemoveWall(w1); - /* std::cout << KRED << "WALL added " << NewWall.toString() << "\n" << RESET ; */ + /* std::cout << KRED << "WALL added " << NewWall1.toString() << "\n" << RESET ; */ /* std::cout << KRED << "WALL removed " << w1.toString() << "\n" << RESET ; */ /* getc(stdin); */ diff --git a/geometry/Building.cpp b/geometry/Building.cpp index e17a968a97c60035633281657a2ee4bbeda9047d..0008a8b86df70388e8593bc51e842fb7d2bcd2c8 100644 --- a/geometry/Building.cpp +++ b/geometry/Building.cpp @@ -461,6 +461,8 @@ const std::vector > Building::GetIntersectionPo // collect pairs of pairs for(auto door: doors) { + // std::cout << "================================\n"; + // std::cout << "door: " << door.toString() << "\n"; PointWall pw1, pw2; int nintersections = 0; auto n = door.NormalVec(); @@ -470,33 +472,86 @@ const std::vector > Building::GetIntersectionPo auto p22 = door.GetPoint2() - n*scaleFactor; auto normalWall1 = Wall(p11, p12); auto normalWall2 = Wall(p21, p22); - /* std::cout << "normal wall 1: " << normalWall1.toString() << "\n"; */ - /* std::cout << "normal wall 2: " << normalWall2.toString() << "\n"; */ + // std::cout << "normal wall 1: " << normalWall1.toString() << "\n"; + // std::cout << "normal wall 2: " << normalWall2.toString() << "\n"; for(auto twall: mytrack) { - /* std::cout << " twall " << twall.toString() << "\n"; */ - Point interPoint1, interPoint2; - auto res = normalWall1.IntersectionWith(twall, interPoint1); - auto res2 = normalWall2.IntersectionWith(twall, interPoint2); - if(res == 1) - { - /* std::cout << "intersection at :" << interPoint1.toString() << "\n"; */ - pw1 = std::make_pair(interPoint1, twall); - nintersections++; + Point interPoint1, interPoint2; + auto res = normalWall1.IntersectionWith(twall, interPoint1); + auto res2 = normalWall2.IntersectionWith(twall, interPoint2); + // std::cout << " res " << res << " res2 " << res2 << "\n"; + if(res == 1) + { + if(!twall.NearlyHasEndPoint(interPoint1)) + { + // std::cout << "res twall " << twall.toString() << "\n"; + pw1 = std::make_pair(interPoint1, twall); + nintersections++; + // std::cout << "intersection at :" << interPoint1.toString() << "\n"; + } + else // end point + { + if(res2 == 0) + { + + // std::cout << "twall " << twall.toString() << "\n"; + // std::cout << "YY intersection 1 at :" << interPoint1.toString() << "\n"; + // std::cout << "YY intersection 2 at :" << interPoint2.toString() << "\n"; + + } + else{ + + // std::cout << "res twall " << twall.toString() << "\n"; + pw1 = std::make_pair(interPoint1, twall); + nintersections++; + // std::cout << "BB: intersection at :" << interPoint1.toString() << "\n"; + } + } } if(res2 == 1) { - pw2 = std::make_pair(interPoint2, twall); - nintersections++; - /* std::cout << "intersection at :" << interPoint2.toString() << "\n"; */ + if(!twall.NearlyHasEndPoint(interPoint2)) + { + // std::cout << "res2 twall " << twall.toString() << "\n"; + pw2 = std::make_pair(interPoint2, twall); + nintersections++; + // std::cout << "intersection at :" << interPoint2.toString() << "\n"; + } + else + { + if(res == 0) + { + // std::cout << "twall " << twall.toString() << "\n"; + // std::cout << "XX intersection 1 at :" << interPoint1.toString() << "\n"; + // std::cout << "XX intersection 2 at :" << interPoint2.toString() << "\n"; + } + else{ + // std::cout << "res2 twall " << twall.toString() << "\n"; + pw2 = std::make_pair(interPoint2, twall); + nintersections++; + // std::cout << "CC intersection at :" << interPoint2.toString() << "\n"; + } + } } + // std::cout << "door: " << door.toString() << ", intersections: " << nintersections << "\n"; + + } // tracks // std::cout << "door: " << door.toString() << ", intersections: " << nintersections << "\n"; + // std::cout << "================================\n"; + if(nintersections == 2) pws.push_back(std::make_pair(pw1, pw2)); + else + { + std::cout << KRED << "Error in GetIntersection. Should be 2 but got " << nintersections << "\n"; + exit(-1); + } }// doors + // getc(stdin); + return pws; } // reset changes made by trainTimeTable[id] diff --git a/geometry/Line.cpp b/geometry/Line.cpp index 21aa72b9816849b0ae0ee4a9e07343dac85c2017..d2bf8a3ff86421591bc559c0b088e32137f21907 100644 --- a/geometry/Line.cpp +++ b/geometry/Line.cpp @@ -366,7 +366,8 @@ int Line::IntersectionWith(const Point& p1, const Point& p2, Point& p3) const double t = (_point1-p1).CrossProduct(s)/(r.CrossProduct(s)); double u = (_point1-p1).CrossProduct(r)/(r.CrossProduct(s)); - if (0>t || t>1) { + + if (-0.05>t || t>1) { return LineIntersectType::NO_INTERSECTION; } @@ -464,6 +465,7 @@ int Line::WichSide(const Point& pt) bool Line::ShareCommonPointWith(const Line& line, Point& P) const { + if (line.GetPoint1()==_point1 || line.GetPoint2()==_point1) { P = _point1; return true; @@ -488,6 +490,23 @@ bool Line::HasEndPoint(const Point& point) const return _point2==point; } +bool Line::NearlyHasEndPoint(const Point& point) const +{ + // std::cout << _point1.toString() << "\n"; + // std::cout << _point2.toString() << "\n"; + // std::cout << point.toString() << "\n"; + + + // std::cout << "--> " << (_point1-point).Norm() << "\n"; + // std::cout << "--> " << (_point2-point).Norm() << "\n"; + // std::cout << "<-- " << J_EPS_DIST << "\n"; + + + if ((_point1-point).Norm() <= J_EPS_DIST) return true; + return ((_point2-point).Norm() <= J_EPS_DIST); +} + + bool Line::IntersectionWithCircle(const Point& centre, double radius /*cm for pedestrians*/) { diff --git a/geometry/Line.h b/geometry/Line.h index 333978b6ce5a9f129347163dd46863ff9ddf0230..d57a9ac3c23dd7b54f60128236a239f73296e0f9 100644 --- a/geometry/Line.h +++ b/geometry/Line.h @@ -134,6 +134,7 @@ public: bool IsInLineSegment(const Point& p) const; bool NearlyInLineSegment(const Point& p) const; + bool NearlyHasEndPoint(const Point& point) const; /** * @return the distance from the line to the point p diff --git a/math/VelocityModel.cpp b/math/VelocityModel.cpp index 7a44515745b9d3ef8e3fdd9d695d8413e4ede3c8..ddec72d148f42c13ab173f5a9a6558a3d5722831 100644 --- a/math/VelocityModel.cpp +++ b/math/VelocityModel.cpp @@ -293,11 +293,11 @@ void VelocityModel::ComputeNextTimeStep(double current, double deltaT, Building* spacings.clear(); //clear for ped p // stuck peds get removed. Warning is thrown. low speed due to jam is omitted. - if(ped->GetTimeInJam() > ped->GetPatienceTime() && ped->GetGlobalTime() > 30 + ped->GetPremovementTime() && + if(ped->GetTimeInJam() > ped->GetPatienceTime() && ped->GetGlobalTime() > 10000 + ped->GetPremovementTime() && std::max(ped->GetMeanVelOverRecTime(), ped->GetV().Norm()) < 0.01 && size == 0 ) // size length of peds neighbour vector { - Log->Write("WARNING:\tped %d with vmean %f has been deleted in room [%i]/[%i] after time %f s (current=%f\n", ped->GetID(), ped->GetMeanVelOverRecTime(), ped->GetRoomID(), ped->GetSubRoomID(), ped->GetGlobalTime(), current); + Log->Write("WARNING:\tped %d with vmean %f has been deleted in room [%i]/[%i] after time %f s (current=%f\n", ped->GetID(), ped->GetMeanVelOverRecTime(), ped->GetRoomID(), ped->GetSubRoomID(), ped->GetGlobalTime(), current); Log->incrementDeletedAgents(); #pragma omp critical(VelocityModel_ComputeNextTimeStep_pedsToRemove) pedsToRemove.push_back(ped);