Commit 7fc0f133 by Mohcine Chraibi

### Fising Strategy 4

parent 74bbbb99
 ... ... @@ -25,11 +25,12 @@ * **/ #include "../math/Mathematics.h" #include "Point.h" //#include "SubRoom.h" #include "../general/Macros.h" #include "Line.h" #include "Wall.h" #include "../IO/OutputHandler.h" ... ... @@ -41,6 +42,9 @@ int Line::_static_UID=0; using namespace std; #define DEBUG 1 /************************************************************ Konstruktoren ************************************************************/ ... ... @@ -521,7 +525,7 @@ std::string Line::toString() const //insteed of a boolian double Line::GetIntersectionDistance(const Line & l) const { #define DEBUG 0 double deltaACy = _point1.GetY() - l.GetPoint1().GetY(); double deltaDCx = l.GetPoint2().GetX() - l.GetPoint1().GetX(); double deltaACx = _point1.GetX() - l.GetPoint1().GetX(); ... ... @@ -531,7 +535,8 @@ double Line::GetIntersectionDistance(const Line & l) const double denominator = deltaBAx * deltaDCy - deltaBAy * deltaDCx; double numerator = deltaACy * deltaDCx - deltaACx * deltaDCy; double infinity =100000; // double infinity =100000; double infinity = std::numeric_limits::infinity(); // the lines are parallel if (denominator == 0.0) { ... ... @@ -568,7 +573,7 @@ double Line::GetIntersectionDistance(const Line & l) const printf("Enter GetIntersection\n"); cout<< "\t" << l.toString() << " intersects with " << toString() < distance is "<< sqrt(dist)<< "... return "<< dist< distance is "<< sqrt(dist)<< "... return (squared) "<< dist<Q, Q is the crossing point // // o P ... ... @@ -591,7 +596,7 @@ double Line::GetIntersectionDistance(const Line & l) const double Line::GetDeviationAngle(const Line & l) const { // const double PI = 3.14159258; #define DEBUG 0 Point P = _point1; Point Goal = _point2; ... ... @@ -624,9 +629,148 @@ double Line::GetDeviationAngle(const Line & l) const } // return the biggest angle between two lanes double Line::GetAngle(const Line & l) const { Point P = _point1; Point Goal = _point2; Point L = l._point1; Point R = l._point2; double angleL, angleR; // we don't need to calculate both angles, but for debugging purposes we do it. angleL = atan((Goal - P).CrossProduct(L - P)/ (Goal - P).ScalarProduct(L - P)); angleR = atan((Goal - P).CrossProduct(R - P)/ (Goal - P).ScalarProduct(R - P)); return (fabs(angleL)>fabs(angleR))?angleL:angleR; } // get the smallest angle that ensures a safe deviation from an // obstacle. Safe means that the rotated line ped--->goal do not // intersect with any line of the obstacle. double Line::GetObstacleDeviationAngle(const std::vector& owalls) const { #if DEBUG printf("Enter GetObstacleDeviationAngle()\n"); #endif Point P = _point1; Point Goal = _point2; Point GL, GR; Point L, R; double minAngle=std::numeric_limits::infinity(), angleL, angleR, angle; Line l, l_large, tmpDirectionL, tmpDirectionR; bool visibleL=true, visibleR=true; // for (unsigned int i = 0; i < owalls.size(); i++) { // l = owalls[i]; for(auto l:owalls){ visibleL = true; visibleR = true; l_large = l.enlarge(3); // 2*ped->GetLargerAxis() L = l_large._point1; R = l_large._point2; angleL = atan((Goal - P).CrossProduct(L - P)/ (Goal - P).ScalarProduct(L - P)); angleR = atan((Goal - P).CrossProduct(R - P)/ (Goal - P).ScalarProduct(R - P)); GL = (Goal-P).Rotate(cos(angleL), sin(angleL))+P; GR = (Goal-P).Rotate(cos(angleR), sin(angleR))+P; tmpDirectionL = Line(P, GL); tmpDirectionR = Line(P, GR); for(auto l_other:owalls){ if (l_other == l) continue; if(tmpDirectionL.IntersectionWith(l_other)){ visibleL = false; break; } } for(auto l_other:owalls){ if (l_other == l) continue; if(tmpDirectionR.IntersectionWith(l_other)){ visibleR = false; break; } } if(visibleR && visibleL){ // both angles are OK. Get // smallest deviation. // prefer right if (almostEqual (angleR, angleL, 0.001)) angle = angleR; angle = (fabs(angleL) < fabs(angleR))?angleL:angleR; } else if(visibleR && !visibleL){ angle = angleR; } else if(!visibleR && visibleL){ angle = angleL; } else{ printf("continue "); printf("VisibleL=%d, VisibleR=%d\n", visibleL, visibleR); continue; // both angles are not OK. check next wall } #if DEBUG printf("\tP=[%f,%f]\n",P.GetX(), P.GetY()); printf("\tGoal=[%f,%f]\n",Goal.GetX(), Goal.GetY()); printf("\tL=[%f,%f]\n",L.GetX(), L.GetY()); printf("\tR=[%f,%f]\n",R.GetX(), R.GetY()); // printf("\t\tdist_Goal_L=%f\n",dist_Goal_L); // printf("\t\tdist_Goal_R=%f\n",dist_Goal_R); printf("VisibleL=%d, VisibleR=%d\n", visibleL, visibleR); printf("\t\t --> angleL=%f\n",angleL); printf("\t\t --> angleR=%f\n",angleR); printf("\t\t --> angle=%f\n",angle); #endif if(fabs(angle) < fabs(minAngle)) minAngle = angle; #if DEBUG printf("\t\t --> minAngle=%f\n", minAngle); #endif }// owalls if(minAngle == std::numeric_limits::infinity()){ printf("WARNING: minAngle ist infinity\n"); getc(stdin); } #if DEBUG printf("Leave GetObstacleDeviationAngle() with angle=%f\n", minAngle); #endif return minAngle; } ... ...
 ... ... @@ -37,6 +37,7 @@ //forward declarations class OutputHandler; class Wall; // external variables extern OutputHandler* Log; ... ... @@ -235,6 +236,8 @@ public: * @return the angle between two lines */ double GetDeviationAngle(const Line& l) const; double GetAngle(const Line & l) const; double GetObstacleDeviationAngle(const std::vector& owalls) const; // double GetAngle(SubRoom s) const; Line enlarge(double d) const; ... ...
 ... ... @@ -63,7 +63,7 @@ euler 0.001 4 1 ... ... @@ -110,8 +110,8 @@ ... ...
 ... ... @@ -64,7 +64,7 @@ int main(int argc, char **argv) if (sim.GetPedsNumber()) { Log->Write("WARNING: \nPedestrians not evacuated [%d] using [%d] threads", Log->Write("WARNING: Pedestrians not evacuated [%d] using [%d] threads", sim.GetPedsNumber(), args->GetMaxOpenMPThreads()); } ... ...
 ... ... @@ -34,6 +34,15 @@ using namespace std; // ok that is not perfect. For a profound discussion see http://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ bool almostEqual (double a, double b, double eps) { // std::cout<< "a=" << a << " b=" << b<< "diff= "<::epsilon(); } /* * Determines the sign of the number x. * Returns -1 if x is negativ else 1. ... ...
 ... ... @@ -33,6 +33,8 @@ #include #include bool almostEqual (double a, double b, double eps); double sign(double x); double sigmoid(double a, double b, double x); ... ...
 ... ... @@ -174,7 +174,7 @@ void Pedestrian::SetV(const Point& v) void Pedestrian::SetV0Norm(double v0,double v0UpStairs, double v0DownStairs) { _ellipse.SetV0(v0); _ellipse.SetV0(v0); _V0DownStairs=v0DownStairs; _V0UpStairs=v0UpStairs; } ... ... @@ -430,7 +430,7 @@ const Point& Pedestrian::GetV0(const Point& target) //new_v0 = delta.NormalizedMolified(); new_v0 = delta.Normalized(); // -------------------------------------- Handover new target t = _newOrientationDelay++ *_deltaT/(1.0+1000* _distToBlockade); t = _newOrientationDelay++ *_deltaT/(1.0+100* _distToBlockade); _V0 = _V0 + (new_v0 - _V0)*( 1 - exp(-t/_tau) ); #if DEBUG ... ...
 ... ... @@ -120,141 +120,278 @@ Point DirectionInRangeBottleneck::GetTarget(Room* room, Pedestrian* ped) const } /** * this strategy is designed to work without Hlines for a general geometry. * First tested for bottlenecks and corners. * number 4 * @param room Pointer * @param ped Pointer to Pedestrians * * @todo Need more tests e.g. for complex geometries. * @todo Need refactoring: Put the WALL and OBS loops in appropriate functions * @return Target (Point) */Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const /// 4 Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const { #define DEBUG 0 using namespace std; const Point& p1 = ped->GetExitLine()->GetPoint1(); const Point& p2 = ped->GetExitLine()->GetPoint2(); Line ExitLine = Line(p1, p2); //Point Lot = ExitLine.LotPoint( ped->GetPos() ); double d = 0.2; //shorten the line by 20 cm Point diff = (p1 - p2).Normalized() * d; Line e_neu = Line(p1 - diff, p2 + diff); Point NextPointOnLine = e_neu.ShortestPoint(ped->GetPos()); Line tmpDirection = Line(ped->GetPos(), NextPointOnLine );//This direction will be rotated if // it intersects a wall || obstacle. // check for intersection with walls // @todo: make a FUNCTION of this #define DEBUG 1 using namespace std; const Point& p1 = ped->GetExitLine()->GetPoint1(); const Point& p2 = ped->GetExitLine()->GetPoint2(); Line ExitLine = Line(p1, p2); //Point Lot = ExitLine.LotPoint( ped->GetPos() ); double d = 0.2; //shorten the line by 20 cm Point diff = (p1 - p2).Normalized() * d; Line e_neu = Line(p1 - diff, p2 + diff); Point NextPointOnLine = e_neu.ShortestPoint(ped->GetPos()); Line tmpDirection = Line(ped->GetPos(), NextPointOnLine );//This direction will be rotated if // it intersects a wall || obstacle. // check for intersection with walls // @todo: make a FUNCTION of this #if DEBUG printf("\n----------\nEnter GetTarget() with PED=%d\n----------\n",ped->GetID()); printf("nextPointOn Line: %f %f\n", NextPointOnLine.GetX(), NextPointOnLine.GetY()); printf("\n----------\nEnter GetTarget() with PED=%d\n----------\n",ped->GetID()); printf("nextPointOn Line: %f %f\n", NextPointOnLine.GetX(), NextPointOnLine.GetY()); #endif double dist; int inear = -1; int iObs = -1; double minDist = 20001; int subroomId = ped->GetSubRoomID(); SubRoom * subroom = room->GetSubRoom(subroomId); //============================ WALLS =========================== const vector& walls = subroom->GetAllWalls(); for (int i = 0; i < subroom->GetNumberOfWalls(); i++) { dist = tmpDirection.GetIntersectionDistance(walls[i]); if (dist < minDist) { inear = i; minDist = dist; double dist; int inear = -1; int iObs = -1; double minDist = 20001; int subroomId = ped->GetSubRoomID(); SubRoom * subroom = room->GetSubRoom(subroomId); //============================ WALLS =========================== const vector& walls = subroom->GetAllWalls(); printf("subroom <%d> has %d walls\n", subroomId , subroom->GetNumberOfWalls()); for (int i = 0; i < subroom->GetNumberOfWalls(); i++) { dist = tmpDirection.GetIntersectionDistance(walls[i]); printf("CHECK %f %f --- %f %f\n===========\n",walls[i].GetPoint1().GetX(),walls[i].GetPoint1().GetY(), walls[i].GetPoint2().GetX(),walls[i].GetPoint2().GetY()); if (dist < minDist) { inear = i; minDist = dist; #if DEBUG printf("Check wall %d. Dist = %f (%f)\n", i, dist, minDist); printf("%f %f --- %f %f\n===========\n",walls[i].GetPoint1().GetX(),walls[i].GetPoint1().GetY(), walls[i].GetPoint2().GetX(),walls[i].GetPoint2().GetY()); printf("Check wall number %d. Dist = %f (%f)\n", i, dist, minDist); printf("%f %f --- %f %f\n===========\n",walls[i].GetPoint1().GetX(),walls[i].GetPoint1().GetY(), walls[i].GetPoint2().GetX(),walls[i].GetPoint2().GetY()); #endif } }//walls //============================ WALLS =========================== //============================ OBST =========================== const vector& obstacles = subroom->GetAllObstacles(); for(unsigned int obs=0; obs& owalls = obstacles[obs]->GetAllWalls(); for (unsigned int i = 0; i < owalls.size(); i++) { dist = tmpDirection.GetIntersectionDistance(owalls[i]); if (dist < minDist) { inear = i; minDist = dist; iObs = obs; // printf("Check OBS:obs=%d, i=%d Dist = %f (%f)\n", obs, i, dist, minDist); // printf("%f %f --- %f %f\n===========\n",owalls[i].GetPoint1().GetX(),owalls[i].GetPoint1().GetY(), owalls[i].GetPoint2().GetX(),owalls[i].GetPoint2().GetY()); } }//walls of obstacle }// obstacles //============================ OBST =========================== double angle = 0; if (inear >= 0) { ped->SetNewOrientationFlag(true); //Mark this pedestrian for next target calculation ped->SetDistToBlockade(minDist); if(iObs >= 0) { const vector& owalls = obstacles[iObs]->GetAllWalls(); angle = tmpDirection.GetDeviationAngle(owalls[inear].enlarge(2*ped->GetLargerAxis())); } }//walls //============================ WALLS =========================== //============================ OBST =========================== const vector& obstacles = subroom->GetAllObstacles(); for(unsigned int obs=0; obs& owalls = obstacles[obs]->GetAllWalls(); for (unsigned int i = 0; i < owalls.size(); i++) { dist = tmpDirection.GetIntersectionDistance(owalls[i]); if (dist < minDist) { inear = i; minDist = dist; iObs = obs; // printf("Check OBS:obs=%d, i=%d Dist = %f (%f)\n", obs, i, dist, minDist); // printf("%f %f --- %f %f\n===========\n",owalls[i].GetPoint1().GetX(),owalls[i].GetPoint1().GetY(), owalls[i].GetPoint2().GetX(),owalls[i].GetPoint2().GetY()); } }//walls of obstacle }// obstacles //============================ OBST =========================== double angle = 0; if (inear >= 0) { ped->SetNewOrientationFlag(true); //Mark this pedestrian for next target calculation ped->SetDistToBlockade(minDist); if(iObs >= 0){ // obstacle is nearest const vector& owalls = obstacles[iObs]->GetAllWalls(); angle = tmpDirection.GetObstacleDeviationAngle(owalls); // angle = tmpDirection.GetDeviationAngle(owalls[inear].enlarge(2*ped->GetLargerAxis())); #if DEBUG printf("COLLISION WITH %f %f --- %f %f\n===========\n",owalls[inear].GetPoint1().GetX(),owalls[inear].GetPoint1().GetY(), owalls[inear].GetPoint2().GetX(),owalls[inear].GetPoint2().GetY()); #endif } else{ angle = tmpDirection.GetDeviationAngle(walls[inear].enlarge(2*ped->GetLargerAxis())); printf("COLLISION WITH OBSTACLE %f %f --- %f %f\n===========\n",owalls[inear].GetPoint1().GetX(),owalls[inear].GetPoint1().GetY(), owalls[inear].GetPoint2().GetX(),owalls[inear].GetPoint2().GetY()); #endif } //iObs else{ // wall is nearest angle = tmpDirection.GetDeviationAngle(walls[inear].enlarge(2*ped->GetLargerAxis())); #if DEBUG printf("COLLISION WITH %f %f --- %f %f\n===========\n",walls[inear].GetPoint1().GetX(),walls[inear].GetPoint1().GetY(), walls[inear].GetPoint2().GetX(),walls[inear].GetPoint2().GetY()); printf("COLLISION WITH WALL %f %f --- %f %f\n===========\n",walls[inear].GetPoint1().GetX(),walls[inear].GetPoint1().GetY(), walls[inear].GetPoint2().GetX(),walls[inear].GetPoint2().GetY()); #endif } }//inear else{ if(ped->GetNewOrientationFlag()){ //this pedestrian could not see the target and now he can see it clearly. // printf("ped->GetNewOrientationFlag()=%d\n",ped->GetNewOrientationFlag());getc(stdin); ped->SetSmoothTurning(); // so the turning should be adapted accordingly. ped->SetNewOrientationFlag(false); } } } //else }//inear else{ if(ped->GetNewOrientationFlag()){ //this pedestrian could not see the target and now he can see it clearly. // printf("ped->GetNewOrientationFlag()=%d\n",ped->GetNewOrientationFlag());getc(stdin); ped->SetSmoothTurning(); // so the turning should be adapted accordingly. ped->SetNewOrientationFlag(false); } } //////////////////////////////////////////////////////////// Point G; if (fabs(angle) > J_EPS) //G = tmpDirection.GetPoint2().Rotate(cos(angle), sin(angle)) ; G = (NextPointOnLine-ped->GetPos()).Rotate(cos(angle), sin(angle))+ped->GetPos() ; else { if(ped->GetNewOrientationFlag()) //this pedestrian could not see the target and now he can see it clearly. ped->SetSmoothTurning(); // so the turning should be adapted accordingly. Point G; if (fabs(angle) > J_EPS) //G = tmpDirection.GetPoint2().Rotate(cos(angle), sin(angle)) ; G = (NextPointOnLine-ped->GetPos()).Rotate(cos(angle), sin(angle))+ped->GetPos() ; else { if(ped->GetNewOrientationFlag()) //this pedestrian could not see the target and now he can see it clearly. ped->SetSmoothTurning(); // so the turning should be adapted accordingly. G = NextPointOnLine; } G = NextPointOnLine; } #if DEBUG printf("inear=%d, iObs=%d, minDist=%f\n", inear, iObs, minDist); printf("PED=%d\n", ped->GetID()); printf ("MC Posx = %.2f, Posy=%.2f, Lot=[%.2f, %.2f]\n", ped->GetPos().GetX(), ped->GetPos().GetY(), NextPointOnLine.GetX(), NextPointOnLine.GetY()); printf("MC p1=[%.2f, %.2f] p2=[%.2f, %.2f]\n", p1.GetX(), p1.GetY(), p2.GetX(), p2.GetY()); printf("angle=%f, G=[%.2f, %.2f]\n", angle, G.GetX(), G.GetY()); printf("\n----------\nLEAVE function with PED=%d\n----------\n",ped->GetID()); printf("inear=%d, iObs=%d, minDist=%f\n", inear, iObs, minDist); printf("PED=%d\n", ped->GetID()); printf ("MC Posx = %.2f, Posy=%.2f, Lot=[%.2f, %.2f]\n", ped->GetPos().GetX(), ped->GetPos().GetY(), NextPointOnLine.GetX(), NextPointOnLine.GetY()); printf("MC p1=[%.2f, %.2f] p2=[%.2f, %.2f]\n", p1.GetX(), p1.GetY(), p2.GetX(), p2.GetY()); printf("angle=%f, G=[%.2f, %.2f]\n", angle, G.GetX(), G.GetY()); printf("\n----------\nLEAVE function with PED=%d\n----------\n",ped->GetID()); #endif // if( ped->GetID() == 12) // fprintf(stderr, "%.2f %.2f %.2f %.2f %f %f %d %.2f %.2f %.2f\n", NextPointOnLine.GetX(), NextPointOnLine.GetY(), // ped->GetPos().GetX(), ped->GetPos().GetY(), G.GetX(), G.GetY(), ped->GetID(), ped->GetV0().GetX(), ped->GetV0().GetY(), ped->GetGlobalTime()); if( ped->GetID() == 21) fprintf(stderr, "%.2f %.2f %.2f %.2f %f %f %d %.2f %.2f %.2f\n", NextPointOnLine.GetX(), NextPointOnLine.GetY(), ped->GetPos().GetX(), ped->GetPos().GetY(), G.GetX(), G.GetY(), ped->GetID(), ped->GetV0().GetX(), ped->GetV0().GetY(), ped->GetGlobalTime()); // this stderr output can be used with plot_desired_velocity.py return G; return G; } /** * this strategy is designed to work without Hlines for a general geometry. * First tested for bottlenecks and corners. * number 4 * @param room Pointer * @param ped Pointer to Pedestrians * * @todo Need more tests e.g. for complex geometries. * @todo Need refactoring: Put the WALL and OBS loops in appropriate functions * @return Target (Point) **/ // */Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const // { // #define DEBUG 1 // using namespace std; // const Point& p1 = ped->GetExitLine()->GetPoint1(); // const Point& p2 = ped->GetExitLine()->GetPoint2(); // Line ExitLine = Line(p1, p2); // //Point Lot = ExitLine.LotPoint( ped->GetPos() ); // double d = 0.2; //shorten the line by 20 cm // Point diff = (p1 - p2).Normalized() * d; // Line e_neu = Line(p1 - diff, p2 + diff); // Point NextPointOnLine = e_neu.ShortestPoint(ped->GetPos()); // Line tmpDirection = Line(ped->GetPos(), NextPointOnLine );//This direction will be rotated if // // it intersects a wall || obstacle. // // check for intersection with walls // // @todo: make a FUNCTION of this // #if DEBUG // printf("\n----------\nEnter GetTarget() with PED=%d\n----------\n",ped->GetID()); // printf("nextPointOn Line: %f %f\n", NextPointOnLine.GetX(), NextPointOnLine.GetY()); // #endif // double dist; // int inear = -1; // int iObs = -1; // double minDist = 20001; // int subroomId = ped->GetSubRoomID(); // SubRoom * subroom = room->GetSubRoom(subroomId); // //============================ WALLS =========================== // const vector& walls = subroom->GetAllWalls(); // for (int i = 0; i < subroom->GetNumberOfWalls(); i++) { // dist = tmpDirection.GetIntersectionDistance(walls[i]); // if (dist < minDist) { // inear = i; // minDist = dist; // #if DEBUG // printf("Check wall %d. Dist = %f (%f)\n", i, dist, minDist); // printf("%f %f --- %f %f\n===========\n",walls[i].GetPoint1().GetX(),walls[i].GetPoint1().GetY(), walls[i].GetPoint2().GetX(),walls[i].GetPoint2().GetY()); // #endif // } // }//walls // //============================ WALLS =========================== // //============================ OBST =========================== // const vector& obstacles = subroom->GetAllObstacles(); // for(unsigned int obs=0; obs& owalls = obstacles[obs]->GetAllWalls(); // for (unsigned int i = 0; i < owalls.size(); i++) { // dist = tmpDirection.GetIntersectionDistance(owalls[i]); // if (dist < minDist) { // inear = i; // minDist = dist; // iObs = obs; // // printf("Check OBS:obs=%d, i=%d Dist = %f (%f)\n", obs, i, dist, minDist); // // printf("%f %f --- %f %f\n===========\n",owalls[i].GetPoint1().GetX(),owalls[i].GetPoint1().GetY(), owalls[i].GetPoint2().GetX(),owalls[i].GetPoint2().GetY()); // } // }//walls of obstacle // }// obstacles