GlobalRouter.cpp 58.4 KB
Newer Older
Ulrich Kemloh's avatar
Ulrich Kemloh committed
1
/**
Weichen's avatar
Weichen committed
2 3
 * \file        GlobalRouter.cpp
 * \date        Dec 15, 2010
4
 * \version     v0.6
5
 * \copyright   <2009-2014> Forschungszentrum J�lich GmbH. All rights reserved.
Ulrich Kemloh's avatar
Ulrich Kemloh committed
6
 *
Weichen's avatar
Weichen committed
7
 * \section License
8
 * This file is part of JuPedSim.
Ulrich Kemloh's avatar
Ulrich Kemloh committed
9
 *
10
 * JuPedSim is free software: you can redistribute it and/or modify
Weichen's avatar
Weichen committed
11
 * it under the terms of the GNU Lesser General Public License as published by
Ulrich Kemloh's avatar
Ulrich Kemloh committed
12 13 14
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 *
15
 * JuPedSim is distributed in the hope that it will be useful,
Ulrich Kemloh's avatar
Ulrich Kemloh committed
16 17 18 19
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
Weichen's avatar
Weichen committed
20
 * You should have received a copy of the GNU Lesser General Public License
21
 * along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
Ulrich Kemloh's avatar
Ulrich Kemloh committed
22
 *
Weichen's avatar
Weichen committed
23
 * \section Description
Ulrich Kemloh's avatar
Ulrich Kemloh committed
24 25
 *
 *
Weichen's avatar
Weichen committed
26 27
 **/

Ulrich Kemloh's avatar
Ulrich Kemloh committed
28 29 30 31

#include "GlobalRouter.h"

#include "AccessPoint.h"
32
#include "Router.h"
33 34 35
#include "NavMesh.h"
#include "DTriangulation.h"

Ulrich Kemloh's avatar
Ulrich Kemloh committed
36 37
#include "../geometry/Building.h"
#include "../pedestrian/Pedestrian.h"
38
#include "../tinyxml/tinyxml.h"
Ulrich Kemloh's avatar
Ulrich Kemloh committed
39 40 41
#include "../geometry/SubRoom.h"
#include "../geometry/Wall.h"
#include "../IO/OutputHandler.h"
Ulrich Kemloh's avatar
Ulrich Kemloh committed
42

43
#include <sstream>
Ulrich Kemloh's avatar
Ulrich Kemloh committed
44 45
#include <cfloat>
#include <fstream>
46 47
#include <iomanip>

Ulrich Kemloh's avatar
Ulrich Kemloh committed
48 49 50

using namespace std;

Ulrich Kemloh's avatar
Ulrich Kemloh committed
51
GlobalRouter::GlobalRouter() :
52
Router()
53 54 55 56 57 58 59
{
     _accessPoints = map<int, AccessPoint*>();
     _map_id_to_index = std::map<int, int>();
     _map_index_to_id = std::map<int, int>();
     _distMatrix = NULL;
     _pathsMatrix = NULL;
     _building = NULL;
60
     _edgeCost=1;
61 62
     //     _rdDistribution = uniform_real_distribution<double> (0,1);
     //     _rdGenerator = default_random_engine(56);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
63 64 65

}

66 67
GlobalRouter::GlobalRouter(int id, RoutingStrategy s) :  Router(id, s)
{
68 69 70 71 72 73 74
     _accessPoints = map<int, AccessPoint*>();
     _map_id_to_index = std::map<int, int>();
     _map_index_to_id = std::map<int, int>();
     _distMatrix = NULL;
     _pathsMatrix = NULL;
     _building = NULL;
     _edgeCost=1;
75

76 77
     //     _rdDistribution = uniform_real_distribution<double> (0,1);
     //     _rdGenerator = default_random_engine(56);
78 79 80

}

81 82 83
GlobalRouter::~GlobalRouter()
{
     if (_distMatrix && _pathsMatrix) {
84
          const int exitsCnt = _building->GetNumberOfGoals() + _building->GetAllGoals().size();
85 86 87 88 89 90 91 92 93 94 95 96 97 98
          for (int p = 0; p < exitsCnt; ++p) {
               delete[] _distMatrix[p];
               delete[] _pathsMatrix[p];
          }

          delete[] _distMatrix;
          delete[] _pathsMatrix;
     }

     map<int, AccessPoint*>::const_iterator itr;
     for (itr = _accessPoints.begin(); itr != _accessPoints.end(); ++itr) {
          delete itr->second;
     }
     _accessPoints.clear();
Ulrich Kemloh's avatar
Ulrich Kemloh committed
99 100
}

101
bool GlobalRouter::Init(Building* building)
102
{
103 104 105 106 107 108 109 110 111 112 113 114 115 116
     //necessary if the init is called several times during the simulation
     Reset();
     Log->Write("INFO:\tInit the Global Router Engine");
     _building = building;
     //only load the information if not previously loaded
     //if(_building->GetNumberOfGoals()==0)
     LoadRoutingInfos(GetRoutingInfoFile());

     if(_generateNavigationMesh)
     {
          //GenerateNavigationMesh();
          TriangulateGeometry();
          //return true;
     }
117

118
     // initialize the distances matrix for the floydwahrshall
119
     int exitsCnt = _building->GetNumberOfGoals() + _building->GetAllGoals().size();
120 121 122 123 124 125 126 127

     _distMatrix = new double*[exitsCnt];
     _pathsMatrix = new int*[exitsCnt];

     for (int i = 0; i < exitsCnt; ++i) {
          _distMatrix[i] = new double[exitsCnt];
          _pathsMatrix[i] = new int[exitsCnt];
     }
128 129

     // Initializing the values
130 131 132 133 134 135 136 137 138 139
     // all nodes are disconnected
     for (int p = 0; p < exitsCnt; ++p) {
          for (int r = 0; r < exitsCnt; ++r) {
               _distMatrix[p][r] = (r == p) ? 0.0 : FLT_MAX;/*0.0*/
               _pathsMatrix[p][r] = p;/*0.0*/
          }
     }

     // init the access points
     int index = 0;
140 141
     for(const auto & itr:_building->GetAllHlines())
     {
142
          //int door=itr->first;
143 144
          int door = itr.second->GetUniqueID();
          Hline* cross = itr.second;
145 146 147 148 149 150 151
          Point centre = cross->GetCentre();
          double center[2] = { centre.GetX(), centre.GetY() };

          AccessPoint* ap = new AccessPoint(door, center);
          ap->SetNavLine(cross);
          char friendlyName[CLENGTH];
          sprintf(friendlyName, "hline_%d_room_%d_subroom_%d", cross->GetID(),
152 153
                    cross->GetRoom1()->GetID(),
                    cross->GetSubRoom1()->GetSubRoomID());
154 155 156 157
          ap->SetFriendlyName(friendlyName);

          // save the connecting sub/rooms IDs
          int id1 = -1;
158 159
          if (cross->GetSubRoom1()) {
               id1 = cross->GetSubRoom1()->GetUID();
160 161 162 163 164 165 166 167 168 169 170
          }

          ap->setConnectingRooms(id1, id1);
          _accessPoints[door] = ap;

          //very nasty
          _map_id_to_index[door] = index;
          _map_index_to_id[index] = door;
          index++;
     }

171 172 173 174
     for(const auto & itr:_building->GetAllCrossings())
     {
          int door = itr.second->GetUniqueID();
          Crossing* cross = itr.second;
175 176 177 178 179 180 181
          const Point& centre = cross->GetCentre();
          double center[2] = { centre.GetX(), centre.GetY() };

          AccessPoint* ap = new AccessPoint(door, center);
          ap->SetNavLine(cross);
          char friendlyName[CLENGTH];
          sprintf(friendlyName, "cross_%d_room_%d_subroom_%d", cross->GetID(),
182 183
                    cross->GetRoom1()->GetID(),
                    cross->GetSubRoom1()->GetSubRoomID());
184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206
          ap->SetFriendlyName(friendlyName);

          // save the connecting sub/rooms IDs
          int id1 = -1;
          if (cross->GetSubRoom1()) {
               id1 = cross->GetSubRoom1()->GetUID();
          }

          int id2 = -1;
          if (cross->GetSubRoom2()) {
               id2 = cross->GetSubRoom2()->GetUID();
          }

          ap->setConnectingRooms(id1, id2);
          _accessPoints[door] = ap;

          //very nasty
          _map_id_to_index[door] = index;
          _map_index_to_id[index] = door;
          index++;
     }


207 208 209 210
     for(const auto & itr:_building->GetAllTransitions())
     {
          int door = itr.second->GetUniqueID();
          Transition* cross = itr.second;
211 212 213 214 215 216 217
          const Point& centre = cross->GetCentre();
          double center[2] = { centre.GetX(), centre.GetY() };

          AccessPoint* ap = new AccessPoint(door, center);
          ap->SetNavLine(cross);
          char friendlyName[CLENGTH];
          sprintf(friendlyName, "trans_%d_room_%d_subroom_%d", cross->GetID(),
218 219
                    cross->GetRoom1()->GetID(),
                    cross->GetSubRoom1()->GetSubRoomID());
220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241
          ap->SetFriendlyName(friendlyName);

          ap->SetClosed(!cross->IsOpen());
          // save the connecting sub/rooms IDs
          int id1 = -1;
          if (cross->GetSubRoom1()) {
               id1 = cross->GetSubRoom1()->GetUID();
          }

          int id2 = -1;
          if (cross->GetSubRoom2()) {
               id2 = cross->GetSubRoom2()->GetUID();
          }

          ap->setConnectingRooms(id1, id2);
          _accessPoints[door] = ap;

          //set the final destination
          if (cross->IsExit() && cross->IsOpen()) {
               ap->SetFinalExitToOutside(true);
               Log->Write("INFO: \tExit to outside found: %d [%s]",ap->GetID(),ap->GetFriendlyName().c_str());
          } else if ((id1 == -1) && (id2 == -1)) {
242
               Log->Write("INFO:\t a final destination outside the geometry was found");
243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258
               ap->SetFinalExitToOutside(true);
          } else if (cross->GetRoom1()->GetCaption() == "outside") {
               ap->SetFinalExitToOutside(true);
          }

          //very nasty
          _map_id_to_index[door] = index;
          _map_index_to_id[index] = door;
          index++;
     }

     // loop over the rooms
     // loop over the subrooms
     // get the transitions in the subrooms
     // and compute the distances

259
     for(auto && itroom:_building->GetAllRooms())
260
     {
261 262
          auto&& room=itroom.second;
          for(const auto & it_sub:room->GetAllSubRooms())
263
          {
264 265
               // The penalty factor should discourage pedestrians to evacuation through rooms
               auto&& sub=it_sub.second;
266 267
               double  penalty=1.0;
               if((sub->GetType()!="floor") && (sub->GetType()!="dA") ) {
268
                    penalty=_edgeCost;
269 270 271 272
               }

               //collect all navigation objects
               vector<NavLine*> allGoals;
273
               const auto & crossings = sub->GetAllCrossings();
274
               allGoals.insert(allGoals.end(), crossings.begin(), crossings.end());
275
               const auto & transitions = sub->GetAllTransitions();
276
               allGoals.insert(allGoals.end(), transitions.begin(),
277
                         transitions.end());
278
               const auto & hlines = sub->GetAllHlines();
279 280 281 282 283
               allGoals.insert(allGoals.end(), hlines.begin(), hlines.end());

               //process the hlines
               //process the crossings
               //process the transitions
284 285
               for (unsigned int n1 = 0; n1 < allGoals.size(); n1++)
               {
286 287 288 289 290 291 292 293 294 295 296 297 298 299 300
                    NavLine* nav1 = allGoals[n1];
                    AccessPoint* from_AP = _accessPoints[nav1->GetUniqueID()];
                    int from_door = _map_id_to_index[nav1->GetUniqueID()];
                    if(from_AP->IsClosed()) continue;

                    for (unsigned int n2 = 0; n2 < allGoals.size(); n2++) {
                         NavLine* nav2 = allGoals[n2];
                         AccessPoint* to_AP = _accessPoints[nav2->GetUniqueID()];
                         if(to_AP->IsClosed()) continue;

                         if (n1 == n2)
                              continue;
                         if (nav1->operator ==(*nav2))
                              continue;

301
                         if (building->IsVisible(nav1->GetCentre(), nav2->GetCentre(), true)) {
302 303
                              int to_door = _map_id_to_index[nav2->GetUniqueID()];
                              _distMatrix[from_door][to_door] = penalty*(nav1->GetCentre()
304
                                        - nav2->GetCentre()).Norm();
305
                              from_AP->AddConnectingAP(
306
                                        _accessPoints[nav2->GetUniqueID()]);
307
                         }
308
                    }
309 310 311 312 313 314 315
               }
          }
     }

     //complete the matrix with the final distances between the exits to the outside and the
     //final marked goals

316 317 318 319
     for (int final_dest:_finalDestinations)
     {
          Goal* goal =_building->GetFinalGoal(final_dest);
          const Wall& line=_building->GetFinalGoal(final_dest)->GetAllWalls()[0];
320 321 322 323
          double center[2] = { goal->GetCentroid()._x, goal->GetCentroid()._y };

          AccessPoint* to_AP = new AccessPoint(line.GetUniqueID(), center);
          to_AP->SetFinalGoalOutside(true);
324 325 326 327 328
          //NavLine* tmpline=new NavLine(line);
          NavLine tmpline(line);
          to_AP->SetNavLine(&tmpline);
          //delete tmpline;

329 330 331 332 333 334 335 336 337 338 339 340 341
          char friendlyName[CLENGTH];
          sprintf(friendlyName, "finalGoal_%d_located_outside", goal->GetId());
          to_AP->SetFriendlyName(friendlyName);
          to_AP->AddFinalDestination(FINAL_DEST_OUT,0.0);
          to_AP->AddFinalDestination(goal->GetId(),0.0);
          _accessPoints[to_AP->GetID()] = to_AP;

          //very nasty
          _map_id_to_index[to_AP->GetID()] = index;
          _map_index_to_id[index] = to_AP->GetID();
          index++;

          //only make a connection to final exit to outside
342 343 344
          for(const auto & itr1: _accessPoints)
          {
               AccessPoint* from_AP = itr1.second;
345 346 347 348 349 350 351
               if(from_AP->GetFinalExitToOutside()==false) continue;
               if(from_AP->GetID()==to_AP->GetID()) continue;
               from_AP->AddConnectingAP(to_AP);
               int from_door= _map_id_to_index[from_AP->GetID()];
               int to_door= _map_id_to_index[to_AP->GetID()];
               // I assume a direct line connection between every exit connected to the outside and
               // any final goal also located outside
352
               _distMatrix[from_door][to_door] = _edgeCost*from_AP->GetNavLine()->DistTo(goal->GetCentroid());
353 354 355 356 357 358 359 360 361 362 363 364 365

               // add a penalty for goals outside due to the direct line assumption while computing the distances
               //if (_distMatrix[from_door][to_door] > 10.0)
               //      _distMatrix[from_door][to_door]*=10;
          }
     }

     //run the floyd warshall algorithm
     FloydWarshall();

     // set the configuration for reaching the outside
     // set the distances to all final APs

366 367 368 369
     for(const auto & itr: _accessPoints)
     {
          AccessPoint* from_AP = itr.second;
          int from_door = _map_id_to_index[itr.first];
370
          if(from_AP->GetFinalGoalOutside()) continue;
371 372

          //maybe put the distance to FLT_MAX
373 374 375 376 377
          if(from_AP->IsClosed()) continue;

          double tmpMinDist = FLT_MAX;
          int tmpFinalGlobalNearestID = from_door;

378 379 380
          for(const auto & itr1: _accessPoints)
          {
               AccessPoint* to_AP = itr1.second;
381 382 383 384
               if(from_AP->GetID()==to_AP->GetID()) continue;
               if(from_AP->GetFinalExitToOutside()) continue;
               //if(from_AP->GetFinalGoalOutside()) continue;

385 386 387
               if (to_AP->GetFinalExitToOutside())
               {
                    int to_door = _map_id_to_index[itr1.first];
388 389 390 391 392 393 394 395 396 397 398
                    if (from_door == to_door)
                         continue;

                    //cout <<" checking final destination: "<< pAccessPoints[j]->GetID()<<endl;
                    double dist = _distMatrix[from_door][to_door];
                    if (dist < tmpMinDist) {
                         tmpFinalGlobalNearestID = to_door;
                         tmpMinDist = dist;
                    }
               }
          }
399

400 401 402
          // in the case it is the final APs
          if (tmpFinalGlobalNearestID == from_door)
               tmpMinDist = 0.0;
403

404 405
          if (tmpMinDist == FLT_MAX) {
               Log->Write(
406 407
                         "ERROR: \tGlobalRouter: There is no visibility path from [%s] to the outside 1\n"
                         "       You can solve this by enabling triangulation.",
408
                         from_AP->GetFriendlyName().c_str());
409
               from_AP->Dump();
410
               return false;
411
          }
412

413 414
          // set the distance to the final destination ( OUT )
          from_AP->AddFinalDestination(FINAL_DEST_OUT, tmpMinDist);
415

416 417
          // set the intermediate path to global final destination
          GetPath(from_door, tmpFinalGlobalNearestID);
418

419 420
          if (_tmpPedPath.size() >= 2) {
               from_AP->AddTransitAPsTo(FINAL_DEST_OUT,
421
                         _accessPoints[_map_index_to_id[_tmpPedPath[1]]]);
422 423
          } else {
               if ((!from_AP->GetFinalExitToOutside())
424 425
                         && (!from_AP->IsClosed()))
               {
426
                    Log->Write(
427 428
                              "ERROR: \tGlobalRouter: There is no visibility path from [%s] to the outside 2\n"
                              "       \tYou can solve this by enabling triangulation.",
429
                              from_AP->GetFriendlyName().c_str());
430
                    from_AP->Dump();
431
                    return false;
432 433 434 435
               }
          }
          _tmpPedPath.clear();
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
436

437 438
     // set the configuration to reach the goals specified in the ini file
     // set the distances to alternative destinations
439

440 441
     for (unsigned int p = 0; p < _finalDestinations.size(); p++) {
          int to_door_uid =
442
                    _building->GetFinalGoal(_finalDestinations[p])->GetAllWalls()[0].GetUniqueID();
443
          int to_door_matrix_index=_map_id_to_index[to_door_uid];
444

445 446 447
          // thats probably a goal located outside the geometry or not an exit from the geometry
          if(to_door_uid==-1) {
               Log->Write(
448
                         "ERROR: \tGlobalRouter: there is something wrong with the final destination [ %d ]\n",
449
                         _finalDestinations[p]);
450
               return false;
451 452
          }

453 454 455
          for(const auto & itr:_accessPoints)
          {
               AccessPoint* from_AP = itr.second;
456 457
               if(from_AP->GetFinalGoalOutside()) continue;
               if(from_AP->IsClosed()) continue;
458
               int from_door_matrix_index = _map_id_to_index[itr.first];
459 460 461 462 463 464 465 466 467 468

               //comment this if you want infinite as distance to unreachable destinations
               double dist = _distMatrix[from_door_matrix_index][to_door_matrix_index];
               from_AP->AddFinalDestination(_finalDestinations[p], dist);

               // set the intermediate path
               // set the intermediate path to global final destination
               GetPath(from_door_matrix_index, to_door_matrix_index);
               if (_tmpPedPath.size() >= 2) {
                    from_AP->AddTransitAPsTo(_finalDestinations[p],
469
                              _accessPoints[_map_index_to_id[_tmpPedPath[1]]]);
470 471 472
               } else {
                    if (((!from_AP->IsClosed()))) {
                         Log->Write(
473 474
                                   "ERROR: \tGlobalRouter: There is no visibility path from [%s] to goal [%d]\n"
                                   "         You can solve this by enabling triangulation.",
475
                                   from_AP->GetFriendlyName().c_str(), _finalDestinations[p]);
476
                         from_AP->Dump();
477
                         return false;
478 479 480 481 482 483 484
                    }
               }
               _tmpPedPath.clear();
          }
     }

     //dumping the complete system
485
     //DumpAccessPoints(-1); //exit(0);
486
     //DumpAccessPoints(-1); exit(0);
487 488
     //vector<string> rooms;
     //rooms.push_back("hall");
489
     //WriteGraphGV("routing_graph.gv",FINAL_DEST_OUT,rooms); exit(0);
490 491
     //WriteGraphGV("routing_graph.gv",1,rooms);
     Log->Write("INFO:\tDone with the Global Router Engine!");
492
     return true;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
493
}
494

495
void GlobalRouter::Reset(){
496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516
     //clean all allocated spaces
     if (_distMatrix && _pathsMatrix) {
          const int exitsCnt = _building->GetNumberOfGoals();
          for (int p = 0; p < exitsCnt; ++p) {
               delete[] _distMatrix[p];
               delete[] _pathsMatrix[p];
          }

          delete[] _distMatrix;
          delete[] _pathsMatrix;
     }

     for (auto itr = _accessPoints.begin(); itr != _accessPoints.end(); ++itr) {
          delete itr->second;
     }

     _accessPoints.clear();
     _tmpPedPath.clear();
     _map_id_to_index.clear();
     _map_index_to_id.clear();
     _mapIdToFinalDestination.clear();
517
}
518

519 520 521 522 523 524 525 526 527 528
void GlobalRouter::SetEdgeCost(double cost)
{
     _edgeCost=cost;
}

double GlobalRouter::GetEdgeCost() const
{
     return _edgeCost;
}

529 530 531 532 533 534 535
void GlobalRouter::GetPath(int i, int j)
{
     if (_distMatrix[i][j] == FLT_MAX)
          return;
     if (i != j)
          GetPath(i, _pathsMatrix[i][j]);
     _tmpPedPath.push_back(j);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
536
}
537

538
bool GlobalRouter::GetPath(Pedestrian* ped, std::vector<NavLine*>& path)
539 540 541 542 543 544 545 546 547 548 549 550 551 552
{
     std::vector<AccessPoint*> aps_path;

     bool done=false;
     int currentNavLine = ped->GetNextDestination();
     if (currentNavLine == -1)
     {
          currentNavLine= GetBestDefaultRandomExit(ped);
     }
     aps_path.push_back(_accessPoints[currentNavLine]);

     int loop_count=1;
     do
     {
553
          const auto & ap=aps_path.back();
554 555 556 557
          int next_dest = ap->GetNearestTransitAPTO(ped->GetFinalDestination());

          if(next_dest==-1) break; //we are done

558
          auto & next_ap= _accessPoints[next_dest];
559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578

          if(next_ap->GetFinalExitToOutside())
          {
               done =true;
          }

          if (! IsElementInVector(aps_path,next_ap))
          {
               aps_path.push_back(next_ap);
          }
          else
          {
               Log->Write("WARNING:\t the line [%d] is already included in the path.");
          }

          //work arround to detect a potential infinte loop.
          if(loop_count++>1000)
          {
               Log->Write("ERROR:\t A path could not be found for pedestrian [%d] going to destination [%d]",ped->GetID(),ped->GetFinalDestination());
               Log->Write("      \t Stuck in an infinite loop [%d].",loop_count);
579
               return false;
580 581 582 583 584 585
          }


     } while (!done);


586
     for(const auto & aps: aps_path)
587 588
          path.push_back(aps->GetNavLine());

589 590
     return true;

591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644
     //collect and return the navigation lines

     //     do
     //     {
     //          SubRoom* sub = _building->GetRoom(ped->GetRoomID())->GetSubRoom(
     //                    ped->GetSubRoomID());
     //
     //          const vector<int>& accessPointsInSubRoom = sub->GetAllGoalIDs();
     //          for (unsigned int i = 0; i < accessPointsInSubRoom.size(); i++) {
     //
     //               int apID = accessPointsInSubRoom[i];
     //               AccessPoint* ap = _accessPoints[apID];
     //
     //               const Point& pt3 = ped->GetPos();
     //               double distToExit = ap->GetNavLine()->DistTo(pt3);
     //
     //               if (distToExit > J_EPS_DIST)
     //                    continue;
     //
     //               //one AP is near actualize destination:
     //               nextDestination = ap->GetNearestTransitAPTO(
     //                         ped->GetFinalDestination());
     //
     //
     //               if (nextDestination == -1) { // we are almost at the exit
     //                    return ped->GetNextDestination();
     //               } else {
     //                    //check that the next destination is in the actual room of the pedestrian
     //                    if (_accessPoints[nextDestination]->isInRange(
     //                              sub->GetUID())==false) {
     //                         //return the last destination if defined
     //                         int previousDestination = ped->GetNextDestination();
     //
     //                         //we are still somewhere in the initialization phase
     //                         if (previousDestination == -1) {
     //                              ped->SetExitIndex(apID);
     //                              ped->SetExitLine(_accessPoints[apID]->GetNavLine());
     //                              return apID;
     //                         } else { // we are still having a valid destination, don't change
     //                              return previousDestination;
     //                         }
     //                    } else { // we have reached the new room
     //                         ped->SetExitIndex(nextDestination);
     //                         ped->SetExitLine(
     //                                   _accessPoints[nextDestination]->GetNavLine());
     //                         return nextDestination;
     //                    }
     //               }
     //          }
     //
     //          // still have a valid destination, so return it
     //          return nextDestination;
     //     }while (!done);
}
645

646
bool GlobalRouter::GetPath(Pedestrian*ped, int goalID, std::vector<SubRoom*>& path)
647 648 649 650 651 652 653 654 655 656 657 658
{

     //clear the global variable holding the paths
     _tmpPedPath.clear();

     int tmpFinalDest=ped->GetFinalDestination();
     ped->SetFinalDestination(goalID);

     //find the nearest APs and start from there
     int next = GetBestDefaultRandomExit(ped);
     if(next==-1) {
          Log->Write("ERROR:\t there is an error in getting the path for ped %d to the goal %d", ped->GetID(),goalID);
659
          return false;
660 661 662 663
     }

     // get the transformed goal_id
     int to_door_uid =
664
               _building->GetFinalGoal(goalID)->GetAllWalls()[0].GetUniqueID();
665 666 667 668 669 670
     int to_door_matrix_index=_map_id_to_index[to_door_uid];
     int from_door_matrix_index=_map_id_to_index[next];

     // thats probably a goal located outside the geometry or not an exit from the geometry
     if(to_door_uid==-1) {
          Log->Write("ERROR: \tGlobalRouter: there is something wrong with final destination [ %d ]\n",goalID);
671
          return false;
672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696
     }

     //populate the line unique id to cross
     GetPath(from_door_matrix_index,to_door_matrix_index);

     for(unsigned int i=0; i<_tmpPedPath.size(); i++) {
          int ap_id= _map_index_to_id[_tmpPedPath[i]];
          int subroom_uid=_accessPoints[ap_id]->GetConnectingRoom1();
          if(subroom_uid==-1) continue;
          SubRoom* sub = _building->GetSubRoomByUID(subroom_uid);
          if (sub && IsElementInVector(path, sub)==false) path.push_back(sub);
     }

     for(unsigned int i=0; i<_tmpPedPath.size(); i++) {
          int ap_id= _map_index_to_id[_tmpPedPath[i]];
          int subroom_uid=_accessPoints[ap_id]->GetConnectingRoom2();
          if(subroom_uid==-1) continue;
          SubRoom* sub = _building->GetSubRoomByUID(subroom_uid);
          if (sub && IsElementInVector(path, sub)==false) path.push_back(sub);
     }

     //clear the global variable holding the paths
     _tmpPedPath.clear();

     ped->SetFinalDestination(tmpFinalDest);
697
     return true;
698 699
     //double distance = _accessPoints[next]->GetDistanceTo(0)+ped->GetDistanceToNextTarget();
     //cout<<"shortest distance to outside: " <<distance<<endl;
700 701
}

Ulrich Kemloh's avatar
Ulrich Kemloh committed
702
/*
Ulrich Kemloh's avatar
Ulrich Kemloh committed
703 704 705 706
 floyd_warshall()
 after calling this function dist[i][j] will the the minimum distance
 between i and j if it exists (i.e. if there's a path between i and j)
 or 0, otherwise
Ulrich Kemloh's avatar
Ulrich Kemloh committed
707
 */
708 709 710 711 712 713 714 715 716 717
void GlobalRouter::FloydWarshall()
{
     const int n = _building->GetNumberOfGoals() + _building->GetAllGoals().size();
     for (int k = 0; k < n; k++)
          for (int i = 0; i < n; i++)
               for (int j = 0; j < n; j++)
                    if (_distMatrix[i][k] + _distMatrix[k][j] < _distMatrix[i][j]) {
                         _distMatrix[i][j] = _distMatrix[i][k] + _distMatrix[k][j];
                         _pathsMatrix[i][j] = _pathsMatrix[k][j];
                    }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
718 719
}

720 721 722 723 724
void GlobalRouter::DumpAccessPoints(int p)
{
     if (p != -1) {
          _accessPoints.at(p)->Dump();
     } else {
725 726 727
          for (const auto & itr: _accessPoints)
          {
               itr.second->Dump();
728 729
          }
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
730 731
}

732 733
int GlobalRouter::FindExit(Pedestrian* ped)
{
734 735 736 737 738 739
     if(_useMeshForLocalNavigation==false)
     {
          std::vector<NavLine*> path;
          GetPath(ped,path);

          //return the next path which is an exit
740
          for(const auto & navLine: path)
741
          {
742 743
               //TODO: only set if the pedestrian is already in the subroom.
               // cuz all lines are returned
744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762
               if(IsCrossing(*navLine) || IsTransition(*navLine))
               {
                    int nav_id= navLine->GetUniqueID();
                    ped->SetExitIndex(nav_id);
                    ped->SetExitLine(navLine);
                    return nav_id;
               }
          }

          //something bad happens
          Log->Write(
                    "ERROR:\t Cannot find a valid destination for ped [%d] located in room [%d] subroom [%d] going to destination [%d]",
                    ped->GetID(), ped->GetRoomID(), ped->GetSubRoomID(),
                    ped->GetFinalDestination());
          return -1;

     }

     // else proceed as usual and return the closest navigation line
Ulrich Kemloh's avatar
Ulrich Kemloh committed
763

764
     int nextDestination = ped->GetNextDestination();
765 766 767 768
     //      if(ped->GetGlobalTime()>80){
     //              ped->Dump(2);
     //              //exit(0);
     //      }
769

770 771
     if (nextDestination == -1) {
          return GetBestDefaultRandomExit(ped);
772

773
     } else {
774

775
          SubRoom* sub = _building->GetRoom(ped->GetRoomID())->GetSubRoom(
776
                    ped->GetSubRoomID());
777

778 779
          for(const auto & apID:sub->GetAllGoalIDs())
          {
780 781 782
               AccessPoint* ap = _accessPoints[apID];
               const Point& pt3 = ped->GetPos();
               double distToExit = ap->GetNavLine()->DistTo(pt3);
783

784 785
               if (distToExit > J_EPS_DIST)
                    continue;
786

787 788
               //one AP is near actualize destination:
               nextDestination = ap->GetNearestTransitAPTO(
789
                         ped->GetFinalDestination());
790 791 792 793 794 795 796


               if (nextDestination == -1) { // we are almost at the exit
                    return ped->GetNextDestination();
               } else {
                    //check that the next destination is in the actual room of the pedestrian
                    if (_accessPoints[nextDestination]->isInRange(
797
                              sub->GetUID())==false) {
798 799 800 801 802 803 804 805 806 807 808 809 810 811
                         //return the last destination if defined
                         int previousDestination = ped->GetNextDestination();

                         //we are still somewhere in the initialization phase
                         if (previousDestination == -1) {
                              ped->SetExitIndex(apID);
                              ped->SetExitLine(_accessPoints[apID]->GetNavLine());
                              return apID;
                         } else { // we are still having a valid destination, don't change
                              return previousDestination;
                         }
                    } else { // we have reached the new room
                         ped->SetExitIndex(nextDestination);
                         ped->SetExitLine(
812
                                   _accessPoints[nextDestination]->GetNavLine());
813
                         return nextDestination;
814
                    }
815 816
               }
          }
817

818 819 820
          // still have a valid destination, so return it
          return nextDestination;
     }
821 822
}

823 824
int GlobalRouter::GetBestDefaultRandomExit(Pedestrian* ped)
{
825 826 827 828 829
     // prob parameters
     //double alpha=0.2000005;
     //double normFactor=0.0;
     //map <int, double> doorProb;

830 831
     // get the opened exits
     SubRoom* sub = _building->GetRoom(ped->GetRoomID())->GetSubRoom(
832
               ped->GetSubRoomID());
833 834 835 836 837


     // get the relevant opened exits
     vector <AccessPoint*> relevantAPs;
     GetRelevantRoutesTofinalDestination(ped,relevantAPs);
838
     //cout<<"relevant APs size:" <<relevantAPs.size()<<endl;
839 840

     int bestAPsID = -1;
841
     double minDistGlobal = FLT_MAX;
842
     double minDistLocal = FLT_MAX;
843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860

     //for (unsigned int i = 0; i < accessPointsInSubRoom.size(); i++) {
     //      int apID = accessPointsInSubRoom[i];
     for(unsigned int g=0; g<relevantAPs.size(); g++) {
          AccessPoint* ap=relevantAPs[g];
          //int exitid=ap->GetID();
          //AccessPoint* ap = _accessPoints[apID];

          if (ap->isInRange(sub->GetUID()) == false)
               continue;
          //check if that exit is open.
          if (ap->IsClosed())
               continue;

          //the line from the current position to the centre of the nav line.
          // at least the line in that direction minus EPS
          const Point& posA = ped->GetPos();
          const Point& posB = ap->GetNavLine()->GetCentre();
861
          const Point& posC = (posB - posA).Normalized() * ((posA - posB).Norm() - J_EPS) + posA;
862 863

          //check if visible
864
          if (sub->IsVisible(posA, posC, true) == false) {
865 866 867 868 869
               ped->RerouteIn(10);
               //ped->Dump(ped->GetID());
               continue;
          }

870 871 872
          double dist1 = ap->GetDistanceTo(ped->GetFinalDestination());
          double dist2 = ap->DistanceTo(posA.GetX(), posA.GetY());
          double dist=dist1+dist2;
873

874 875 876 877
          //        doorProb[ap->GetID()]= exp(-alpha*dist);
          //        normFactor += doorProb[ap->GetID()];


878 879 880 881
          //          if (dist < minDistGlobal) {
          //               bestAPsID = ap->GetID();
          //               minDistGlobal = dist;
          //          }
882

883
          // normalize the probs
884 885 886 887 888 889 890 891 892 893 894 895 896
          //    double randomVar = _rdDistribution(_rdGenerator);
          //
          //    for (const auto & it = doorProb.begin(); it!=doorProb.end(); ++it){
          //        it->second =  it->second / normFactor;
          //    }
          //
          //    double cumProb= doorProb.begin()->second;
          //    const auto & it = doorProb.begin();
          //    while(cumProb<randomVar) {
          //        it++;
          //        cumProb+=it->second;
          //    }
          //    bestAPsID=it->first;
897

898
          //very useful for short term decisions
899
          // if two doors are feasible to the final destination without much differences
900
          // in the distances, then the nearest is preferred.
901 902
          if(( (dist-minDistGlobal) / (dist+minDistGlobal)) < CBA_THRESHOLD)
          {
903 904 905 906 907
               if (dist2 < minDistLocal) {
                    bestAPsID = ap->GetID();
                    minDistGlobal = dist;
                    minDistLocal= dist2;
               }
908 909 910

          } else {

911 912 913 914 915
               if (dist < minDistGlobal) {
                    bestAPsID = ap->GetID();
                    minDistGlobal = dist;
                    minDistLocal=dist2;
               }
916
          }
917 918 919 920 921 922 923 924 925
     }

     if (bestAPsID != -1) {
          ped->SetExitIndex(bestAPsID);
          ped->SetExitLine(_accessPoints[bestAPsID]->GetNavLine());
          return bestAPsID;
     } else {
          if (_building->GetRoom(ped->GetRoomID())->GetCaption() != "outside")
               Log->Write(
926 927 928 929
                         "ERROR:\t GetBestDefaultRandomExit() \nCannot find valid destination for ped [%d] "
                         "located in room [%d] subroom [%d] going to destination [%d]",
                         ped->GetID(), ped->GetRoomID(), ped->GetSubRoomID(),
                         ped->GetFinalDestination());
930 931
          return -1;
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
932 933 934
}


935 936
void GlobalRouter::GetRelevantRoutesTofinalDestination(Pedestrian *ped, vector<AccessPoint*>& relevantAPS)
{
Ulrich Kemloh's avatar
Ulrich Kemloh committed
937

938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954 955 956 957
     Room* room=_building->GetRoom(ped->GetRoomID());
     SubRoom* sub=room->GetSubRoom(ped->GetSubRoomID());

     // This is best implemented by closing one door and checking if there is still a path to outside
     // and itereating over the others.
     // It might be time consuming, you many pre compute and cache the results.
     if(sub->GetAllHlines().size()==0)
     {
          const vector<int>& goals=sub->GetAllGoalIDs();
          //filter to keep only the emergencies exits.

          for(unsigned int g1=0; g1<goals.size(); g1++) {
               AccessPoint* ap=_accessPoints[goals[g1]];
               bool relevant=true;
               for(unsigned int g2=0; g2<goals.size(); g2++) {
                    if(goals[g2]==goals[g1]) continue; // always skip myself
                    if(ap->GetNearestTransitAPTO(ped->GetFinalDestination())==goals[g2]) {
                         // crossings only
                         relevant=false;
                         break;
958
                    }
959 960 961 962 963 964 965 966
               }
               if(relevant==true) {
                    //only if not closed
                    if(ap->IsClosed()==false)
                         relevantAPS.push_back(ap);
                    //cout<<"relevant APs:" <<ap->GetID()<<endl;
               }
          }
967

968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985
     }
     //quick fix for extra hlines
     // it should be safe now to delete the first preceding if block
     else
     {
          const vector<int>& goals=sub->GetAllGoalIDs();
          for(unsigned int g1=0; g1<goals.size(); g1++)
          {
               AccessPoint* ap=_accessPoints[goals[g1]];

               //check for visibility
               //the line from the current position to the centre of the nav line.
               // at least the line in that direction minus EPS
               const Point& posA = ped->GetPos();
               const Point& posB = ap->GetNavLine()->GetCentre();
               const Point& posC = (posB - posA).Normalized() * ((posA - posB).Norm() - J_EPS) + posA;

               //check if visible
986
               if (sub->IsVisible(posA, posC, true) == false)
987 988 989 990 991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006
               {
                    continue;
               }

               bool relevant=true;
               for(unsigned int g2=0; g2<goals.size(); g2++)
               {
                    if(goals[g2]==goals[g1]) continue; // always skip myself
                    if(ap->GetNearestTransitAPTO(ped->GetFinalDestination())==goals[g2])
                    {

                         //pointing only to the one i dont see
                         //the line from the current position to the centre of the nav line.
                         // at least the line in that direction minus EPS
                         AccessPoint* ap2=_accessPoints[goals[g2]];
                         const Point& posA = ped->GetPos();
                         const Point& posB = ap2->GetNavLine()->GetCentre();
                         const Point& posC = (posB - posA).Normalized()* ((posA - posB).Norm() - J_EPS) + posA;

                         //it points to a destination that I can see anyway
1007
                         if (sub->IsVisible(posA, posC, true) == true)
1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028
                         {
                              relevant=false;
                         }

                         break;
                    }
               }
               if(relevant==true)
               {
                    if(ap->IsClosed()==false)
                         relevantAPS.push_back(ap);
                    //cout<<"relevant APs:" <<ap->GetID()<<endl;
               }
          }
     }
     //    if(relevantAPS.size()==2){
     //         cout<<"alternative wege: "<<relevantAPS.size()<<endl;
     //         cout<<"ap1: "<<relevantAPS[0]->GetID()<<endl;
     //         cout<<"ap2: "<<relevantAPS[1]->GetID()<<endl;
     //         getc(stdin);
     //    }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
1029
}
1030

1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047
SubRoom* GlobalRouter::GetCommonSubRoom(Crossing* c1, Crossing* c2)
{
     SubRoom* sb11 = c1->GetSubRoom1();
     SubRoom* sb12 = c1->GetSubRoom2();
     SubRoom* sb21 = c2->GetSubRoom1();
     SubRoom* sb22 = c2->GetSubRoom2();

     if (sb11 == sb21)
          return sb11;
     if (sb11 == sb22)
          return sb11;
     if (sb12 == sb21)
          return sb12;
     if (sb12 == sb22)
          return sb12;

     return NULL;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
1048 1049
}

Ulrich Kemloh's avatar
Ulrich Kemloh committed
1050
void GlobalRouter::WriteGraphGV(string filename, int finalDestination,
1051
          const vector<string> rooms_captions)
1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062
{
     ofstream graph_file(filename.c_str());
     if (graph_file.is_open() == false) {
          Log->Write("Unable to open file" + filename);
          return;
     }

     //header
     graph_file << "## Produced by OPS_GCFM" << endl;
     //graph_file << "##comand: \" sfdp -Goverlap=prism -Gcharset=latin1"<<filename <<"| gvmap -e | neato -Ecolor=\"#55555522\" -n2 -Tpng > "<< filename<<".png \""<<endl;
     graph_file << "##Command to produce the output: \"neato -n -s -Tpng "
1063
               << filename << " > " << filename << ".png\"" << endl;
1064 1065 1066 1067 1068
     graph_file << "digraph OPS_GCFM_ROUTING {" << endl;
     graph_file << "overlap=scale;" << endl;
     graph_file << "splines=false;" << endl;
     graph_file << "fontsize=20;" << endl;
     graph_file
1069 1070
     << "label=\"Graph generated by the routing engine for destination: "
     << finalDestination << "\"" << endl;
1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082

     vector<int> rooms_ids = vector<int>();

     if (rooms_captions.empty()) {
          // then all rooms should be printed
          for (int i = 0; i < _building->GetNumberOfRooms(); i++) {
               rooms_ids.push_back(i);
          }

     } else {
          for (unsigned int i = 0; i < rooms_captions.size(); i++) {
               rooms_ids.push_back(
1083
                         _building->GetRoom(rooms_captions[i])->GetID());
1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101
          }
     }

     for (map<int, AccessPoint*>::const_iterator itr = _accessPoints.begin();
               itr != _accessPoints.end(); ++itr) {

          AccessPoint* from_AP = itr->second;

          int from_door = from_AP->GetID();

          // check for valid room
          NavLine* nav = from_AP->GetNavLine();
          int room_id = -1;

          if (dynamic_cast<Crossing*>(nav) != NULL) {
               room_id = ((Crossing*) (nav))->GetRoom1()->GetID();

          } else if (dynamic_cast<Hline*>(nav) != NULL) {
1102
               room_id = ((Hline*) (nav))->GetRoom1()->GetID();
1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127

          } else if (dynamic_cast<Transition*>(nav) != NULL) {
               room_id = ((Transition*) (nav))->GetRoom1()->GetID();

          } else {
               cout << "WARNING: Unkown navigation line type" << endl;
               continue;
          }

          if (IsElementInVector(rooms_ids, room_id) == false)
               continue;

          double px = from_AP->GetCentre().GetX();
          double py = from_AP->GetCentre().GetY();
          //graph_file << from_door <<" [shape=ellipse, pos=\""<<px<<", "<<py<<" \"] ;"<<endl;
          //graph_file << from_door <<" [shape=ellipse, pos=\""<<px<<","<<py<<"\" ];"<<endl;

          //const vector<AccessPoint*>& from_aps = from_AP->GetConnectingAPs();
          const vector<AccessPoint*>& from_aps = from_AP->GetTransitAPsTo(
                    finalDestination);

          if (from_aps.size() == 0) {

               if (from_AP->GetFinalExitToOutside()) {
                    graph_file << from_door << " [pos=\"" << px << ", " << py
1128 1129
                              << " \", style=filled, color=green,fontsize=5] ;"
                              << endl;
1130 1131 1132
                    //                              graph_file << from_door <<" [width=\"0.41\", height=\"0.31\",fixedsize=false,pos=\""<<px<<", "<<py<<" \", style=filled, color=green,fontsize=4] ;"<<endl;
               } else {
                    graph_file << from_door << " [pos=\"" << px << ", " << py
1133
                              << " \", style=filled, color=red,fontsize=5] ;" << endl;
1134 1135 1136 1137 1138 1139 1140 1141 1142 1143 1144 1145 1146 1147
                    //                              graph_file << from_door <<" [width=\"0.41\", height=\"0.31\",fixedsize=false,pos=\""<<px<<", "<<py<<" \", style=filled, color=red,fontsize=4] ;"<<endl;
               }
          } else {
               // check that all connecting aps are contained in the room_ids list
               // if not marked as sink.
               bool isSink = true;
               for (unsigned int j = 0; j < from_aps.size(); j++) {
                    NavLine* nav = from_aps[j]->GetNavLine();
                    int room_id = -1;

                    if (dynamic_cast<Crossing*>(nav) != NULL) {
                         room_id = ((Crossing*) (nav))->GetRoom1()->GetID();

                    } else if (dynamic_cast<Hline*>(nav) != NULL) {
1148
                         room_id = ((Hline*) (nav))->GetRoom1()->GetID();
1149 1150 1151 1152 1153 1154 1155 1156

                    } else if (dynamic_cast<Transition*>(nav) != NULL) {
                         room_id = ((Transition*) (nav))->GetRoom1()->GetID();

                    } else {
                         cout << "WARNING: Unkown navigation line type" << endl;
                         continue;
                    }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
1157

1158 1159 1160 1161 1162
                    if (IsElementInVector(rooms_ids, room_id) == true) {
                         isSink = false;
                         break;
                    }
               }
1163

1164 1165 1166
               if (isSink) {
                    //graph_file << from_door <<" [width=\"0.3\", height=\"0.21\",fixedsize=false,pos=\""<<px<<", "<<py<<" \" ,style=filled, color=green, fontsize=4] ;"<<endl;
                    graph_file << from_door << " [pos=\"" << px << ", " << py
1167 1168
                              << " \" ,style=filled, color=blue, fontsize=5] ;"
                              << endl;
1169 1170 1171
               } else {
                    //graph_file << from_door <<" [width=\"0.3\", height=\"0.231\",fixedsize=false, pos=\""<<px<<", "<<py<<" \", fontsize=4] ;"<<endl;
                    graph_file << from_door << " [pos=\"" << px << ", " << py
1172 1173
                              << " \", style=filled, color=yellow, fontsize=5] ;"
                              << endl;
1174 1175
               }
          }
1176

1177
     }
1178

1179
     //connections
1180 1181 1182
     for (const auto & itr: _accessPoints)
     {
          AccessPoint* from_AP = itr.second;
1183
          int from_door = from_AP->GetID();
1184

1185

1186

1187 1188
          NavLine* nav = from_AP->GetNavLine();
          int room_id = -1;
1189

1190 1191
          if (dynamic_cast<Crossing*>(nav) != NULL) {
               room_id = ((Crossing*) (nav))->GetRoom1()->GetID();
1192

1193