SubRoom.cpp 34.6 KB
Newer Older
1
/**
Weichen's avatar
Weichen committed
2 3
 * \file        SubRoom.cpp
 * \date        Oct 8, 2010
4
 * \version     v0.6
Weichen's avatar
Weichen committed
5
 * \copyright   <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
6
 *
Weichen's avatar
Weichen committed
7
 * \section License
8 9 10
 * This file is part of JuPedSim.
 *
 * 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
12 13 14 15 16 17 18 19
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 *
 * JuPedSim is distributed in the hope that it will be useful,
 * 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 22
 * along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
 *
Weichen's avatar
Weichen committed
23
 * \section Description
24 25
 *
 *
Weichen's avatar
Weichen committed
26
 **/
Ulrich Kemloh's avatar
Ulrich Kemloh committed
27

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

#include "Point.h"
#include "Line.h"
#include "Wall.h"
32
#include "Obstacle.h"
Ulrich Kemloh's avatar
Ulrich Kemloh committed
33
#include "SubRoom.h"
34
#include "Transition.h"
35
#include "Hline.h"
Ulrich Kemloh's avatar
Ulrich Kemloh committed
36

37

38 39 40
#ifdef _SIMULATOR
#include "../pedestrian/Pedestrian.h"
#endif //_SIMULATOR
41

42
#include <cmath>
43 44

using namespace std;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
45 46 47 48 49

/************************************************************
 SubRoom
 ************************************************************/

50
int SubRoom::_static_uid=0;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
51

52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72
SubRoom::SubRoom()
{
     _id = -1;
     _roomID=-1;
     _walls = vector<Wall > ();
     _poly = vector<Point > ();
     _obstacles=vector<Obstacle*> ();

     _crossings = vector<Crossing*>();
     _transitions = vector<Transition*>();
     _hlines = vector<Hline*>();

     _planeEquation[0]=0.0;
     _planeEquation[1]=0.0;
     _planeEquation[2]=0.0;
     _cosAngleWithHorizontalPlane=0;

     _goalIDs = vector<int> ();
     _area = 0.0;
     _closed=false;
     _uid = _static_uid++;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
73 74
}

75 76
SubRoom::~SubRoom()
{
Ulrich Kemloh's avatar
Ulrich Kemloh committed
77 78
     for (unsigned int i = 0; i < _obstacles.size(); i++)
     {
79 80 81
          delete _obstacles[i];
     }
     _obstacles.clear();
Ulrich Kemloh's avatar
Ulrich Kemloh committed
82 83 84
}


85 86 87
void SubRoom::SetSubRoomID(int ID)
{
     _id = ID;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
88
}
89 90 91
void SubRoom::SetClosed(double closed)
{
     _closed = closed;
92
}
Ulrich Kemloh's avatar
Ulrich Kemloh committed
93

94 95 96
void SubRoom::SetRoomID(int ID)
{
     _roomID = ID;
97 98
}

99 100 101
int SubRoom::GetSubRoomID() const
{
     return _id;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
102 103
}

104 105 106
double SubRoom::GetClosed() const
{
     return _closed;
107
}
Ulrich Kemloh's avatar
Ulrich Kemloh committed
108

109
// unique identifier for this subroom
110 111 112 113
int SubRoom::GetUID() const
{
     return _uid;
     //return pRoomID * 1000 + pID;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
114 115
}

116 117 118
double SubRoom::GetArea() const
{
     return _area;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
119 120
}

121 122 123
int SubRoom::GetRoomID() const
{
     return _roomID;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
124 125
}

126 127 128
const vector<Wall>& SubRoom::GetAllWalls() const
{
     return _walls;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
129 130
}

131 132 133
const vector<Point>& SubRoom::GetPolygon() const
{
     return _poly;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
134 135
}

136 137 138
const vector<Obstacle*>& SubRoom::GetAllObstacles() const
{
     return _obstacles;
139 140
}

141 142
int SubRoom::GetNumberOfGoalIDs() const
{
143
     return (int)_goalIDs.size();
Ulrich Kemloh's avatar
Ulrich Kemloh committed
144 145
}

146 147 148
const vector<int>& SubRoom::GetAllGoalIDs() const
{
     return _goalIDs;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
149 150 151
}


152
bool SubRoom::AddWall(const Wall& w)
153
{
154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169
     //check for duplicated walls
     for(auto&& w1: _walls)
     {
          if(w==w1)
          {
               Log->Write("ERROR:\t Duplicate wall found in Room/Subroom %d/%d  %s",_roomID,_id ,w.toString().c_str());
               Log->Write("ERROR:\t will not be added");
               exit(EXIT_FAILURE);
               //return false;
          }
     }
     //checking for wall chunks.
     if(w.Length()<J_TOLERANZ)
     {
          Log->Write("ERROR:\t Wall too small (length = %lf) found in Room/Subroom %d/%d  %s",w.Length(),_roomID,_id ,w.toString().c_str());
          Log->Write("ERROR:\t will not be added");
Ulrich Kemloh's avatar
Ulrich Kemloh committed
170
          //exit(EXIT_FAILURE);
171 172
          //return false;
     }
173
     _walls.push_back(w);
174
     return true;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
175 176 177
}


178 179 180 181
void SubRoom::AddObstacle(Obstacle* obs)
{
     _obstacles.push_back(obs);
     CheckObstacles();
182 183
}

Ulrich Kemloh's avatar
Ulrich Kemloh committed
184

185 186 187
void SubRoom::AddGoalID(int ID)
{
     _goalIDs.push_back(ID);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
188 189
}

190 191 192 193
void SubRoom::AddCrossing(Crossing* line)
{
     _crossings.push_back(line);
     _goalIDs.push_back(line->GetUniqueID());
194 195
}

196 197 198 199
void SubRoom::AddTransition(Transition* line)
{
     _transitions.push_back(line);
     _goalIDs.push_back(line->GetUniqueID());
200 201
}

Ulrich Kemloh's avatar
Ulrich Kemloh committed
202 203 204 205 206 207 208 209
void SubRoom::AddNeighbor(SubRoom* sub)
{
     if(sub and (IsElementInVector(_neighbors, sub)==false))
     {
          _neighbors.push_back(sub);
     }
}

210 211
void SubRoom::AddHline(Hline* line)
{
Ulrich Kemloh's avatar
Ulrich Kemloh committed
212 213 214 215
     for(unsigned int i=0;i<_hlines.size();i++)
     {
          if (line->GetID()==_hlines[i]->GetID())
          {
216 217 218 219
               Log->Write("INFO:\tskipping duplicate hline [%d] with id [%d]",_id,line->GetID());
               return;
          }
     }
220

221 222
     _hlines.push_back(line);
     _goalIDs.push_back(line->GetUniqueID());
223 224
}

225 226 227
const vector<Crossing*>& SubRoom::GetAllCrossings() const
{
     return _crossings;
228 229
}

230 231 232
const vector<Transition*>& SubRoom::GetAllTransitions() const
{
     return _transitions;
233 234
}

235 236 237
const vector<Hline*>& SubRoom::GetAllHlines() const
{
     return _hlines;
238
}
239

Ulrich Kemloh's avatar
Ulrich Kemloh committed
240 241 242 243 244
const std::vector<SubRoom*>& SubRoom::GetNeighbors() const
{
     return _neighbors;
}

245 246 247
const Crossing* SubRoom::GetCrossing(int i) const
{
     return _crossings[i];
248 249
}

250 251 252
const Transition* SubRoom::GetTransition(int i) const
{
     return _transitions[i];
253 254
}

255 256 257
const Hline* SubRoom::GetHline(int i) const
{
     return _hlines[i];
258 259
}

260 261 262 263 264 265 266 267 268 269
void SubRoom::RemoveGoalID(int ID)
{
     for (unsigned int i=0; i<_goalIDs.size(); i++) {
          if(_goalIDs[i]==ID) {
               Log->Write("Removing goal");
               _goalIDs.erase(_goalIDs.begin()+i);
               return;
          }
     }
     Log->Write("There is no goal with that id to remove");
Ulrich Kemloh's avatar
Ulrich Kemloh committed
270 271 272
}


273 274 275 276 277 278 279 280
void SubRoom::CalculateArea()
{
     double sum = 0;
     int n = (int) _poly.size();
     for (int i = 0; i < n; i++) {
          sum += (_poly[i].GetY() + _poly[(i + 1) % n].GetY())*(_poly[i].GetX() - _poly[(i + 1) % n].GetX());
     }
     _area=(0.5 * fabs(sum));
Ulrich Kemloh's avatar
Ulrich Kemloh committed
281 282
}

283 284 285 286 287 288 289 290 291 292
Point SubRoom::GetCentroid() const
{
     double px=0,py=0;
     double signedArea = 0.0;
     double x0 = 0.0; // Current vertex X
     double y0 = 0.0; // Current vertex Y
     double x1 = 0.0; // Next vertex X
     double y1 = 0.0; // Next vertex Y
     double a = 0.0;  // Partial signed area

293
     if(_poly.size()==0) return Point(0,0);
294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321
     // For all vertices except last
     unsigned int i=0;
     for (i=0; i<_poly.size()-1; ++i) {
          x0 = _poly[i].GetX();
          y0 = _poly[i].GetY();
          x1 = _poly[i+1].GetX();
          y1 = _poly[i+1].GetY();
          a = x0*y1 - x1*y0;
          signedArea += a;
          px += (x0 + x1)*a;
          py += (y0 + y1)*a;
     }

     // Do last vertex
     x0 = _poly[i].GetX();
     y0 = _poly[i].GetY();
     x1 = _poly[0].GetX();
     y1 = _poly[0].GetY();
     a = x0*y1 - x1*y0;
     signedArea += a;
     px += (x0 + x1)*a;
     py += (y0 + y1)*a;

     signedArea *= 0.5;
     px /= (6.0*signedArea);
     py /= (6.0*signedArea);

     return Point(px,py);
322 323
}

Ulrich Kemloh's avatar
Ulrich Kemloh committed
324 325
vector<Wall> SubRoom::GetVisibleWalls(const Point & position)
{
326 327
#define DEBUG 0
     std::vector<Wall> visible_walls;
328
     bool wall_is_vis;
329
     Point nearest_point;
330 331 332
#if DEBUG
     printf("\n---------------------------\nEnter GetVisiblewalls\n");
#endif
333
     for (auto w:_walls){
334
          // nearest_point = w.ShortestPoint(position);
335
          wall_is_vis = IsVisible(w, position);
336
          if(wall_is_vis){
337 338 339 340 341
#if DEBUG
               printf("  GetVisibleWalls: Wall (%f, %f)--(%f, %f)\n",w.GetPoint1().GetX(), w.GetPoint1().GetY(),w.GetPoint2().GetX(), w.GetPoint2().GetY() );
               printf("  GetVisibleWalls: Ped position (%f, %f)\n",position.GetX(), position.GetY());
               printf("  GetVisibleWalls: wall is visible? = %d\n",wall_is_vis);
#endif
342
               visible_walls.push_back(w);
343
          }
344
     }
345 346
#if DEBUG
     printf("Leave GetVisiblewalls with %d visible walls\n------------------------------\n",visible_walls.size());
347
#endif
348 349 350
     return visible_walls;
}

351 352
// like ped_is_visible() but here we can exclude checking intersection
// with the same wall. This function should check if <position> can see the <Wall>
353
bool SubRoom::IsVisible(const Line &wall, const Point &position)
354 355 356 357 358 359 360 361
{
     // printf("\tEnter wall_is_visible\n");
     // printf(" \t  Wall (%f, %f)--(%f, %f)\n",wall.GetPoint1().GetX(), wall.GetPoint1().GetY(),wall.GetPoint2().GetX(), wall.GetPoint2().GetY() );

     bool wall_is_vis = true;
     // Point nearest_point =  wall.ShortestPoint(position);
     // in cases where nearest_point is endPoint of Wall, the wall becomes visible even if it is not..
     //  try with the center. If it is not visible then the wall is definitly not.
Ulrich Kemloh's avatar
Ulrich Kemloh committed
362 363
     const Point& nearest_point =  wall.GetCentre();

364 365
     // printf("\t\t center of wall %f, %f\n",nearest_point.GetX(), nearest_point.GetY());
     Line ped_wall = Line(position, nearest_point);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
366 367
     for (auto& w:_walls)
     {
368 369
          if(w == wall) //ignore wall
               continue;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
370 371
          if(wall_is_vis  && ped_wall.IntersectionWith(w))
          {
372 373 374 375 376
               // fprintf (stdout, "\t\t Wall_is_visible: INTERSECTION WALL  L1_P1(%.2f, %.2f), L1_P2(%.2f, %.2f), WALL(%.2f, %.2f)---(%.2f, %.2f)\n", ped_wall.GetPoint1().GetX(),ped_wall.GetPoint1().GetY(), ped_wall.GetPoint2().GetX(), ped_wall.GetPoint2().GetY(), w.GetPoint1().GetX(),w.GetPoint1().GetY(),w.GetPoint2().GetX(),w.GetPoint2().GetY());
               wall_is_vis = false;
          }
     }

Ulrich Kemloh's avatar
Ulrich Kemloh committed
377 378 379 380
     for(const auto& obst: _obstacles)
     {
          for(const auto& w: obst->GetAllWalls())
          {
381 382 383 384 385 386 387 388 389 390 391 392 393 394
               if(wall_is_vis && ped_wall.IntersectionWith(w)){
                    // fprintf (stdout, "\t\t Wall_is_visible INTERSECTION OBS; L1_P1(%.2f, %.2f), L1_P2(%.2f, %.2f), L2_P1(%.2f, %.2f) L2_P2(%.2f, %.2f)\n", w.GetPoint1().GetX(), w.GetPoint1().GetY(), w.GetPoint2().GetX(), w.GetPoint2().GetY(), ped_wall.GetPoint1().GetX(), ped_wall.GetPoint1().GetY(), ped_wall.GetPoint2().GetX(), ped_wall.GetPoint2().GetY());
                    wall_is_vis = false;
               }
          }
     }

     // printf("\tLeave wall_is_visible with %d\n", wall_is_vis);
     return wall_is_vis;
}

// p1 and p2 are supposed to be pedestrian's positions. This function does not work properly 
// for visibility checks with walls, since the line connecting the pedestrian's position 
// with the nearest point on the wall IS intersecting with the wall.
395
bool SubRoom::IsVisible(const Point& p1, const Point& p2, bool considerHlines)
396
{
397
     //check intersection with Walls
398 399 400 401 402 403 404
     for(const auto& wall : _walls)
     {
          if (wall.IntersectionWith(p1, p2))
          {
               return false;
          }
     }
405

406
     // printf("\t\t -- ped_is_visible; check obstacles\n");
407
     //check intersection with obstacles
408 409 410 411 412 413 414 415 416 417
     for ( const auto& obstacle : _obstacles)
     {
          for ( const auto& wall : obstacle->GetAllWalls())
          {
               if (wall.IntersectionWith(p1, p2))
               {
                    return false;
               }
          }
     }
418 419

     if(considerHlines)
420 421 422 423 424 425 426 427 428
     {
          for(const auto& hline: _hlines)
          {

               if(hline->IsInLineSegment(p1)|| hline->IsInLineSegment(p2)) continue;
               if(hline->IntersectionWith(p1, p2))
               {
                    return false;
               }
429
          }
430
     }
431

432
     // printf("\t\tLeave ped_is_visible with %d\n",temp);
433
     return true;
434
}
Mohcine Chraibi's avatar
Mohcine Chraibi committed
435

436
// this is the case if they share a transition or crossing
Ulrich Kemloh's avatar
Ulrich Kemloh committed
437 438 439
bool SubRoom::IsDirectlyConnectedWith(SubRoom* sub) const
{
     return IsElementInVector(_neighbors, sub);
440 441 442 443 444 445 446 447 448
}

void SubRoom::SetPlanEquation(double A, double B, double C)
{
     _planeEquation[0]=A;
     _planeEquation[1]=B;
     _planeEquation[2]=C;
     //compute and cache the cosine of angle with the plane z=h
     _cosAngleWithHorizontalPlane= (1.0/sqrt(A*A+B*B+1));
449 450
}

451
const double* SubRoom::GetPlaneEquation() const
452
{
453
     return _planeEquation;
454 455
}

456 457 458 459
double SubRoom::GetElevation(const Point& p) const
{
     return _planeEquation[0] * p._x + _planeEquation[1] * p._y + _planeEquation[2];
}
Ulrich Kemloh's avatar
Ulrich Kemloh committed
460

461 462 463
double SubRoom::GetCosAngleWithHorizontal() const
{
     return _cosAngleWithHorizontalPlane;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
464

465
}
Ulrich Kemloh's avatar
Ulrich Kemloh committed
466

467
bool SubRoom::CheckObstacles()
468
{
Ulrich Kemloh's avatar
Ulrich Kemloh committed
469 470 471 472 473 474 475
     for(const auto& wall: _walls)
     {
          for(const auto& obst: _obstacles)
          {
               if(obst->IntersectWithLine(wall))
               {
                    Log->Write("ERROR: \tthe obstacle id [%d] is intersection with subroom [%d]",obst->GetId(),_id);
476 477
                    Log->Write("     : \tthe triangulation will not work.");
                    return false;
478 479 480
               }
          }
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
481 482 483 484 485 486 487 488 489 490 491 492 493 494 495

     return true;
}

bool SubRoom::Overlapp(const std::vector<Line*>& goals) const
{
     for(const auto& wall: _walls)
      {
          for(const auto& goal:goals)
          {
               if (wall.Overlapp(*goal)) return true;
          }
      }

     return false;
496 497
}

498
bool SubRoom::SanityCheck()
499
{
Ulrich Kemloh's avatar
Ulrich Kemloh committed
500 501 502 503
     if(_obstacles.size()==0)
     {
          if((IsConvex()==false) && (_hlines.size()==0))
          {
504 505 506 507 508 509
               Log->Write("WARNING:\t Room [%d] Subroom [%d] is not convex!",_roomID,_id);
               Log->Write("\t\t you might consider adding extra hlines in your routing.xml file");
          } else {
               // everything is fine
          }
     } else {
Ulrich Kemloh's avatar
Ulrich Kemloh committed
510 511
          if(_hlines.size()==0)
          {
512 513 514 515 516 517
               Log->Write("WARNING:\t you have obstacles in room [%d] Subroom [%d]!",_roomID,_id);
               Log->Write("\t\t you might consider adding extra hlines in your routing.xml file");
          } else {
               // everything is fine
          }
     }
518 519 520
     //check if there are overlapping walls
     for (auto&& w1: _walls)
     {
521 522
          bool connected=false;

523 524 525
          for (auto&& w2: _walls)
          {
               if (w1==w2) continue;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
526
               if(w1.Overlapp(w2))
527
               {
528
                    Log->Write("ERROR: Overlapping between walls %s and %s ",w1.toString().c_str(),w2.toString().c_str());
Ulrich Kemloh's avatar
Ulrich Kemloh committed
529 530
                    exit(EXIT_FAILURE);
                    //return false;
531
               }
532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565
               connected=connected or w1.ShareCommonPointWith(w2);
          }
          //overlapping with lines
          for(auto&& hline: _hlines)
          {
               if(w1.Overlapp(*hline))
               {
                    Log->Write("ERROR: Overlapping between wall %s and  Hline %s ",w1.toString().c_str(),hline->toString().c_str());
                    exit(EXIT_FAILURE);
                    //return false;
               }
          }
          //overlaping with crossings
          for(auto&& c: _crossings)
          {
               if(w1.Overlapp(*c))
               {
                    Log->Write("ERROR: Overlapping between wall %s and  crossing %s ",w1.toString().c_str(),c->toString().c_str());
                    exit(EXIT_FAILURE);
                    //return false;
               }
               connected=connected or w1.ShareCommonPointWith(*c);
          }
          //overlaping with transitions
          for(auto&& t: _transitions)
          {
               if(w1.Overlapp(*t))
               {
                    Log->Write("ERROR: Overlapping between wall %s and  transition %s ",w1.toString().c_str(),t->toString().c_str());
                    exit(EXIT_FAILURE);
                    //return false;
               }
               connected=connected or w1.ShareCommonPointWith(*t);
          }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
566

567 568 569
          if(not connected)
          {
               Log->Write("ERROR: loose wall found %s  in Room/Subroom %d/%d",w1.toString().c_str(),_roomID,_id);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
570
               exit(EXIT_FAILURE);
571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611
               //return false;
          }
     }

     //check overlaping between hline and crossings
     for(auto&& h: _hlines)
     {
          for(auto&& c: _crossings)
          {
               if(h->Overlapp(*c))
               {
                    Log->Write("ERROR: Overlapping between hline %s and  crossing %s ",h->toString().c_str(),c->toString().c_str());
                    exit(EXIT_FAILURE);
                    //return false;
               }
          }
     }
     //check overlaping between hlines and transitions
     for(auto&& h: _hlines)
     {
          for(auto&& t: _transitions)
          {
               if(h->Overlapp(*t))
               {
                    Log->Write("ERROR: Overlapping between hline %s and  transition %s ",h->toString().c_str(),t->toString().c_str());
                    exit(EXIT_FAILURE);
                    //return false;
               }
          }
     }
     //check overlaping between transitions and crossings
     for(auto&& c: _crossings)
     {
          for(auto&& t: _transitions)
          {
               if(c->Overlapp(*t))
               {
                    Log->Write("ERROR: Overlapping between crossing %s and  transition %s ",c->toString().c_str(),t->toString().c_str());
                    exit(EXIT_FAILURE);
                    //return false;
               }
612 613
          }
     }
614

615
     return true;
616 617 618
}

///http://stackoverflow.com/questions/471962/how-do-determine-if-a-polygon-is-complex-convex-nonconvex
619 620
bool SubRoom::IsConvex()
{
621
     unsigned int hsize=(unsigned int)_poly.size();
622 623 624 625 626 627 628 629 630 631 632 633
     unsigned int pos=0;
     unsigned int neg=0;

     if(hsize==0) {
          Log->Write("WARNING:\t cannot check empty polygon for convexification");
          Log->Write("WARNING:\t Did you forget to tall ConvertLineToPoly() ?");
          return false;
     }

     for(unsigned int i=0; i<hsize; i++) {
          Point vecAB= _poly[(i+1)%hsize]-_poly[i%hsize];
          Point vecBC= _poly[(i+2)%hsize]-_poly[(i+1)%hsize];
634
          double det= vecAB.Determinant(vecBC);
635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651
          if(fabs(det)<J_EPS) det=0.0;

          if(det<0.0) {
               neg++;
          } else if(det>0.0) {
               pos++;
          } else {
               pos++;
               neg++;
          }

     }

     if ( (pos==hsize ) || (neg==hsize) ) {
          return true;
     }
     return false;
652 653 654
}

///http://stackoverflow.com/questions/9473570/polygon-vertices-clockwise-or-counterclockwise/
655 656 657 658 659 660 661
bool SubRoom::IsClockwise()
{
     if(_poly.size()<3) {
          Log->Write("ERROR:\tYou need at least 3 vertices to check for orientation. Subroom ID [%d]");
          return false;
          //exit(EXIT_FAILURE);
     }
662

663 664
     Point vecAB= _poly[1]-_poly[0];
     Point vecBC= _poly[2]-_poly[1];
665

666
     double det= vecAB.Determinant(vecBC);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
667
     return (fabs(det)<J_EPS);
668 669
}

670

Ulrich Kemloh's avatar
Ulrich Kemloh committed
671 672 673
/************************************************************
 NormalSubRoom
 ************************************************************/
674 675 676 677 678 679 680 681 682 683 684 685
NormalSubRoom::NormalSubRoom() : SubRoom()
{
}


NormalSubRoom::~NormalSubRoom()
{
}

string NormalSubRoom::WriteSubRoom() const
{
     string s;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
686 687
     for(auto&& w: _walls)
     {
688 689 690 691
          string geometry;
          char wall[CLENGTH] = "";
          geometry.append("\t\t<wall>\n");
          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\" zPos=\"%.2f\"/>\n",
692 693 694
                    (w.GetPoint1().GetX()) * FAKTOR,
                    (w.GetPoint1().GetY()) * FAKTOR,
                    GetElevation(w.GetPoint1())*FAKTOR);
695 696
          geometry.append(wall);
          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\" zPos=\"%.2f\"/>\n",
697 698 699
                    (w.GetPoint2().GetX()) * FAKTOR,
                    (w.GetPoint2().GetY()) * FAKTOR,
                    GetElevation(w.GetPoint2())*FAKTOR);
700 701 702 703 704 705
          geometry.append(wall);
          geometry.append("\t\t</wall>\n");

          s.append(geometry);
          //s.append(GetWall(j).Write());
     }
706

707
     //add the subroom caption
708
     const Point& pos = GetCentroid();
709
     char tmp[CLENGTH];
710 711
     sprintf(tmp, "\t\t<label centerX=\"%.2f\" centerY=\"%.2f\" centerZ=\"%.2f\" text=\"%d\" color=\"100\" />\n"
               , pos.GetX() * FAKTOR, pos.GetY() * FAKTOR, GetElevation(pos)*FAKTOR,GetSubRoomID());
712 713 714
     s.append(tmp);

     //write the obstacles
715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740
     for (auto&& obst : GetAllObstacles())
     {
          for (auto&& w: obst->GetAllWalls())
          {
               char wall[CLENGTH] = "";
               s.append("\t\t<wall>\n");
               sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\" zPos=\"%.2f\"/>\n",
                         (w.GetPoint1().GetX()) * FAKTOR,
                         (w.GetPoint1().GetY()) * FAKTOR,
                         GetElevation(w.GetPoint1())*FAKTOR);
               s.append(wall);
               sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\" zPos=\"%.2f\"/>\n",
                         (w.GetPoint2().GetX()) * FAKTOR,
                         (w.GetPoint2().GetY()) * FAKTOR,
                         GetElevation(w.GetPoint2())*FAKTOR);
               s.append(wall);
               s.append("\t\t</wall>\n");
          }

          const Point& pos = obst->GetCentroid();

          //add the obstacle caption
          char tmp[CLENGTH];
          sprintf(tmp, "\t\t<label centerX=\"%.2f\" centerY=\"%.2f\" centerZ=\"%.2f\" text=\"%d\" color=\"100\" />\n"
                    , pos.GetX() * FAKTOR, pos.GetY() * FAKTOR,GetElevation(pos)*FAKTOR ,obst->GetId());
          s.append(tmp);
741 742 743 744 745 746 747 748 749 750 751
     }

     return s;
}

string NormalSubRoom::WritePolyLine() const
{
     string s;
     char tmp[CLENGTH];

     s.append("\t<Obstacle closed=\"1\" boundingbox=\"0\" class=\"1\">\n");
Ulrich Kemloh's avatar
Ulrich Kemloh committed
752 753 754
     for(const auto& p: _poly)
     {
          sprintf(tmp, "\t\t<Vertex p_x = \"%.2lf\" p_y = \"%.2lf\"/>\n",p.GetX()* FAKTOR,p.GetY()* FAKTOR);
755 756 757 758 759
          s.append(tmp);
     }
     s.append("\t</Obstacle>\n");

     //write the obstacles
Ulrich Kemloh's avatar
Ulrich Kemloh committed
760 761 762
     for(auto&& obst: _obstacles)
     {
          s.append(obst->Write());
763 764 765 766 767 768 769
     }
     return s;
}

void NormalSubRoom::WriteToErrorLog() const
{
     Log->Write("\t\tNormal SubRoom:\n");
Ulrich Kemloh's avatar
Ulrich Kemloh committed
770 771
     for(auto&& w: _walls)
     {
772 773 774 775
          w.WriteToErrorLog();
     }
}

Ulrich Kemloh's avatar
Ulrich Kemloh committed
776
bool NormalSubRoom::ConvertLineToPoly(const vector<Line*>& goals)
777 778 779 780 781 782
{
     vector<Line*> copy;
     vector<Point> tmpPoly;
     Point point;
     Line* line;
     // Alle Linienelemente in copy speichern
Ulrich Kemloh's avatar
Ulrich Kemloh committed
783 784 785
     for(auto& w: _walls)
     {
          copy.push_back(&w);
786
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
787

788 789 790
     // Transitions und Crossings sind in goal abgespeichert
     copy.insert(copy.end(), goals.begin(), goals.end());

Ulrich Kemloh's avatar
Ulrich Kemloh committed
791 792 793 794 795 796
     if(Overlapp(goals))
     {
          Log->Write("ERROR:\t Overlapping between walls and goals");
          return false;
     }

797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820
     line = copy[0];
     tmpPoly.push_back(line->GetPoint1());
     point = line->GetPoint2();
     copy.erase(copy.begin());
     // Polygon aus allen Linen erzeugen
     for (int i = 0; i < (int) copy.size(); i++) {
          line = copy[i];
          if ((point - line->GetPoint1()).Norm() < J_TOLERANZ) {
               tmpPoly.push_back(line->GetPoint1());
               point = line->GetPoint2();
               copy.erase(copy.begin() + i);
               // von vorne suchen
               i = -1;
          } else if ((point - line->GetPoint2()).Norm() < J_TOLERANZ) {
               tmpPoly.push_back(line->GetPoint2());
               point = line->GetPoint1();
               copy.erase(copy.begin() + i);
               // von vorne suchen
               i = -1;
          }
     }
     if ((tmpPoly[0] - point).Norm() > J_TOLERANZ) {
          char tmp[CLENGTH];
          sprintf(tmp, "ERROR: \tNormalSubRoom::ConvertLineToPoly(): SubRoom %d Room %d Anfangspunkt ungleich Endpunkt!!!\n"
821 822
                    "\t(%f, %f) != (%f, %f)\n", GetSubRoomID(), GetRoomID(), tmpPoly[0].GetX(), tmpPoly[0].GetY(), point.GetX(),
                    point.GetY());
823 824 825
          Log->Write(tmp);
          sprintf(tmp, "ERROR: \tDistance between the points: %lf !!!\n", (tmpPoly[0] - point).Norm());
          Log->Write(tmp);
826
          return false;
827 828
     }
     _poly = tmpPoly;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847


     //check if all walls and goals were used in the polygon
     for (const auto& w: _walls)
          {
               if( ! (  IsElementInVector(_poly,w.GetPoint1()) and
                         IsElementInVector(_poly,w.GetPoint2())) )
               {
                    Log->Write("ERROR:\t Wall was not used during polygon creation for subroom: %s",w.toString().c_str());
                    return false;
               }
          }

          for (const auto& g: goals)
          {
               if( ! (  IsElementInVector(_poly,g->GetPoint1()) and
                         IsElementInVector(_poly,g->GetPoint2())) )
               {
                    Log->Write("ERROR:\t goal was not used during polygon creation for subroom: %s",g->toString().c_str());
848
                    //return false;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
849 850
               }
          }
851
     return true;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
852 853 854 855 856 857 858
}


// private Funktionen

// gibt zuruck in welchen Quadranten vertex liegt, wobei hitPos der Koordinatenursprung ist

859 860 861
int NormalSubRoom::WhichQuad(const Point& vertex, const Point& hitPos) const
{
     return (vertex.GetX() > hitPos.GetX()) ? ((vertex.GetY() > hitPos.GetY()) ? 1 : 4) :
862
               ((vertex.GetY() > hitPos.GetY()) ? 2 : 3);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
863 864 865 866 867

}

// x-Koordinate der Linie von einer Eccke zur nächsten

868 869 870
double NormalSubRoom::Xintercept(const Point& point1, const Point& point2, double hitY) const
{
     return (point2.GetX() - (((point2.GetY() - hitY) * (point1.GetX() - point2.GetX())) /
871
               (point1.GetY() - point2.GetY())));
Ulrich Kemloh's avatar
Ulrich Kemloh committed
872 873 874 875 876
}


// neue Version auch für konkave Polygone

877 878 879 880 881 882 883
bool NormalSubRoom::IsInSubRoom(const Point& ped) const
{
     short edge, first, next;
     short quad, next_quad, delta, total;

     /////////////////////////////////////////////////////////////
     edge = first = 0;
884
     quad = (short) WhichQuad(_poly[edge], ped);
885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908
     total = 0; // COUNT OF ABSOLUTE SECTORS CROSSED
     /* LOOP THROUGH THE VERTICES IN A SECTOR */
     do {
          next = (edge + 1) % _poly.size();
          next_quad = WhichQuad(_poly[next], ped);
          delta = next_quad - quad; // HOW MANY QUADS HAVE I MOVED

          // SPECIAL CASES TO HANDLE CROSSINGS OF MORE THEN ONE
          //QUAD

          switch (delta) {
          case 2: // IF WE CROSSED THE MIDDLE, FIGURE OUT IF IT
               //WAS CLOCKWISE OR COUNTER
          case -2: // US THE X POSITION AT THE HIT POINT TO
               // DETERMINE WHICH WAY AROUND
               if (Xintercept(_poly[edge], _poly[next], ped._y) > ped._x)
                    delta = -(delta);
               break;
          case 3: // MOVING 3 QUADS IS LIKE MOVING BACK 1
               delta = -1;
               break;
          case -3: // MOVING BACK 3 IS LIKE MOVING FORWARD 1
               delta = 1;
               break;
909
          default:break;
910 911 912 913 914 915 916 917 918 919 920 921
          }
          /* ADD IN THE DELTA */
          total += delta;
          quad = next_quad; // RESET FOR NEXT STEP
          edge = next;
     } while (edge != first);

     /* AFTER ALL IS DONE IF THE TOTAL IS 4 THEN WE ARE INSIDE */
     if (abs(total) == 4)
          return true;
     else
          return false;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
922 923 924 925 926 927
}

/************************************************************
 Stair
 ************************************************************/

928 929 930 931
Stair::Stair() : NormalSubRoom()
{
     pUp = Point();
     pDown = Point();
Ulrich Kemloh's avatar
Ulrich Kemloh committed
932 933 934
}


935 936
Stair::~Stair()
{
Ulrich Kemloh's avatar
Ulrich Kemloh committed
937 938 939 940
}

// Setter-Funktionen

941 942 943
void Stair::SetUp(const Point & p)
{
     pUp = p;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
944 945
}

946 947 948
void Stair::SetDown(const Point & p)
{
     pDown = p;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
949 950 951 952
}

// Getter-Funktionen

953 954 955
const Point & Stair::GetUp() const
{
     return pUp;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
956 957
}

958 959 960
const Point & Stair::GetDown() const
{
     return pDown;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
961 962
}

963 964 965
string Stair::WriteSubRoom() const
{
     string s;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
966 967
     for(auto&& w: _walls)
     {
968 969 970 971
          string geometry;
          char wall[CLENGTH] = "";
          geometry.append("\t\t<wall>\n");
          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\" zPos=\"%.2f\"/>\n",
972 973 974
                    (w.GetPoint1().GetX()) * FAKTOR,
                    (w.GetPoint1().GetY()) * FAKTOR,
                    GetElevation(w.GetPoint1())*FAKTOR);
975 976
          geometry.append(wall);
          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\" zPos=\"%.2f\"/>\n",
977 978 979
                    (w.GetPoint2().GetX()) * FAKTOR,
                    (w.GetPoint2().GetY()) * FAKTOR,
                    GetElevation(w.GetPoint2())*FAKTOR);
980 981 982 983 984 985 986 987 988 989
          geometry.append(wall);
          geometry.append("\t\t</wall>\n");

          s.append(geometry);
          //s.append(w.Write());
     }
     //Line tmp = Line(GetUp(), GetDown());
     // s.append(tmp.Write());
     Point pos = GetCentroid();
     char tmp_c[CLENGTH];
990
     sprintf(tmp_c, "\t\t<sphere centerX=\"%.2f\" centerY=\"%.2f\" centerZ=\"%.2f\" radius=\"%.2f\" color=\"100\" />\n"
991
               , GetUp().GetX() * FAKTOR, GetUp().GetY() * FAKTOR,GetElevation(GetUp())*FAKTOR, 0.2*FAKTOR);
992 993 994 995
     s.append(tmp_c);

     //add the subroom caption
     sprintf(tmp_c, "\t\t<label centerX=\"%.2f\" centerY=\"%.2f\" centerZ=\"%.2f\" text=\"%d\" color=\"100\" />\n"
996
               , pos.GetX() * FAKTOR, pos.GetY() * FAKTOR,GetElevation(pos)*FAKTOR ,GetSubRoomID());
997 998 999 1000 1001 1002 1003
     s.append(tmp_c);

     return s;
}

string Stair::WritePolyLine() const
{
Mohcine Chraibi's avatar
Mohcine Chraibi committed
1004

1005 1006
     string s;
     char tmp[CLENGTH];
Mohcine Chraibi's avatar
Mohcine Chraibi committed
1007

1008 1009
     s.append("\t<Obstacle closed=\"1\" boundingbox=\"0\" class=\"1\">\n");
     for (unsigned int j = 0; j < _poly.size(); j++) {
1010
          sprintf(tmp, "\t\t<Vertex p_x = \"%.2lf\" p_y = \"%.2lf\"/>\n",_poly[j].GetX()* FAKTOR,_poly[j].GetY()* FAKTOR);
1011 1012 1013
          s.append(tmp);
     }
     s.append("\t</Obstacle>\n");
Mohcine Chraibi's avatar
Mohcine Chraibi committed
1014

1015 1016 1017 1018
     //write the obstacles
     for( unsigned int j=0; j<GetAllObstacles().size(); j++) {
          s.append(GetAllObstacles()[j]->Write());
     }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
1019

1020 1021 1022 1023 1024
     return s;
}
void Stair::WriteToErrorLog() const
{
     Log->Write("\t\tStair:\n");
Ulrich Kemloh's avatar
Ulrich Kemloh committed
1025 1026
     for(auto&& w: _walls)
     {
1027 1028
          w.WriteToErrorLog();
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
1029 1030
}

1031 1032 1033 1034 1035 1036 1037 1038 1039
/* prüft ob die Punkte p1, p2 und p3 auf einer Linie liegen, oder eine Ecke bilden.
 * Dabei liegt p2 IMMER in der Mitte und entspricht aktPoint
 * */
const Point* Stair::CheckCorner(const Point** otherPoint, const Point** aktPoint, const Point* nextPoint)
{
     Point l1 = **otherPoint - **aktPoint;
     Point l2 = *nextPoint - **aktPoint;
     const Point* rueck = NULL;
     // Punkte bilden eine Linie
1040
     if (fabs(fabs(l1.ScalarProduct(l2) / (l1.Norm() * l2.Norm())) - 1) < 0.1) {
1041 1042 1043 1044 1045 1046 1047 1048 1049
          *aktPoint = nextPoint;
     } else { // aktPoint/p2 ist eine Ecke
          rueck = *aktPoint;
          *otherPoint = *aktPoint;
          *aktPoint = nextPoint;
     }
     return rueck;
}

Ulrich Kemloh's avatar
Ulrich Kemloh committed
1050
bool Stair::ConvertLineToPoly(const vector<Line*>& goals)
1051 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062
{
     //return NormalSubRoom::ConvertLineToPoly(goals);
     vector<Line*> copy;
     vector<Point> orgPoly = vector<Point > ();
     const Point* aktPoint;
     const Point* otherPoint;
     const Point* nextPoint;
     const Point* firstAktPoint;
     const Point* firstOtherPoint;
     Line *nextLine;

     // Alle Linienelemente in copy speichern
Ulrich Kemloh's avatar
Ulrich Kemloh committed
1063 1064 1065
     for(auto& w: _walls)
     {
          copy.push_back(&w);
1066
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
1067

1068 1069 1070
     // Transitions und Crossings sind in goal abgespeichert
     copy.insert(copy.end(), goals.begin(), goals.end());

Ulrich Kemloh's avatar
Ulrich Kemloh committed
1071 1072 1073 1074 1075 1076
     if(Overlapp(goals))
     {
          Log->Write("ERROR:\t Overlapping between walls and goals");
          return false;
     }

1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111
     aktPoint = &copy[0]->GetPoint1();
     firstAktPoint = aktPoint;
     otherPoint = &copy[0]->GetPoint2();
     firstOtherPoint = otherPoint;
     copy.erase(copy.begin());

     // Polygon aus allen Linen erzeugen
     for (int i = 0; i < (int) copy.size(); i++) {
          nextLine = copy[i];
          nextPoint = NULL;
          if ((*aktPoint - nextLine->GetPoint1()).Norm() < J_TOLERANZ) {
               nextPoint = &nextLine->GetPoint2();
          } else if ((*aktPoint - nextLine->GetPoint2()).Norm() < J_TOLERANZ) {
               nextPoint = &nextLine->GetPoint1();
          }
          if (nextPoint != NULL) {
               const Point* rueck = CheckCorner(&otherPoint, &aktPoint, nextPoint);
               if (rueck != NULL)
                    orgPoly.push_back(*rueck);
               copy.erase(copy.begin() + i);
               i = -1; // von vorne suchen
          }
     }
     if ((*aktPoint - *firstOtherPoint).Norm() < J_TOLERANZ) {
          const Point* rueck = CheckCorner(&otherPoint, &aktPoint, firstAktPoint);
          if (rueck != NULL)
               orgPoly.push_back(*rueck);
     } else {
          char tmp[CLENGTH];
          double x1, y1, x2, y2;
          x1 = firstOtherPoint->GetX();
          y1 = firstOtherPoint->GetY();
          x2 = aktPoint->GetX();
          y2 = aktPoint->GetY();
          sprintf(tmp, "ERROR: \tStair::ConvertLineToPoly(): SubRoom %d Room %d Anfangspunkt ungleich Endpunkt!!!\n"
1112
                    "\t(%f, %f) != (%f, %f)\n", GetSubRoomID(), GetRoomID(), x1, y1, x2, y2);
1113
          Log->Write(tmp);
1114
          return false;
1115 1116 1117 1118 1119
     }

     if (orgPoly.size() != 4) {
          char tmp[CLENGTH];
          sprintf(tmp, "ERROR: \tStair::ConvertLineToPoly(): Stair %d Room %d ist kein Viereck!!!\n"
1120
                    "Anzahl Ecken: %d\n", GetSubRoomID(), (int)GetRoomID(), (int)orgPoly.size());
1121
          Log->Write(tmp);
1122
          return false;
1123 1124 1125
     }
     vector<Point> neuPoly = (orgPoly);
     // ganz kleine Treppen (nur eine Stufe) nicht
Ulrich Kemloh's avatar
Ulrich Kemloh committed
1126 1127 1128 1129
     if ((neuPoly[0] - neuPoly[1]).Norm() > 0.9 && (neuPoly[1] - neuPoly[2]).Norm() > 0.9)
     {
          for (int i1 = 0; i1 < (int) orgPoly.size(); i1++)
          {
1130 1131 1132
               unsigned long i2 = (i1 + 1) % orgPoly.size();
               unsigned long i3 = (i2 + 1) % orgPoly.size();
               unsigned long i4 = (i3 + 1) % orgPoly.size();
1133 1134 1135 1136 1137 1138 1139
               Point p1 = neuPoly[i1];
               Point p2 = neuPoly[i2];
               Point p3 = neuPoly[i3];
               Point p4 = neuPoly[i4];
               Point l1 = p2 - p1;
               Point l2 = p3 - p2;

Ulrich Kemloh's avatar
Ulrich Kemloh committed
1140 1141
               if (l1.Norm() < l2.Norm())
               {
1142 1143 1144 1145 1146 1147 1148
                    neuPoly[i2] = neuPoly[i2] + l1.Normalized() * 2 * J_EPS_GOAL;
                    l2 = p3 - p4;
                    neuPoly[i3] = neuPoly[i3] + l2.Normalized() * 2 * J_EPS_GOAL;
               }
          }
     }
     _poly = neuPoly;
1149

Ulrich Kemloh's avatar
Ulrich Kemloh committed
1150 1151 1152 1153 1154 1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166 1167 1168 1169 1170
     //check if all walls and goals were used in the polygon
//     for (const auto& w: _walls)
//     {
//          if( ! (  IsElementInVector(_poly,w.GetPoint1()) and
//                    IsElementInVector(_poly,w.GetPoint2())) )
//          {
//               Log->Write("ERROR:\t Wall was not used during polygon creation for stair: %s",w.toString().c_str());
//               return false;
//          }
//     }
//
//     for (const auto& g: goals)
//     {
//          if( ! (  IsElementInVector(_poly,g->GetPoint1()) and
//                    IsElementInVector(_poly,g->GetPoint2())) )
//          {
//               Log->Write("ERROR:\t goal was not used during polygon creation for stair: %s",g->toString().c_str());
//               return false;
//          }
//     }

1171
     return true;
1172 1173 1174 1175 1176 1177 1178
}

bool Stair::IsInSubRoom(const Point& ped) const
{
     bool rueck = false;
     int N = (int) _poly.size();
     int sum = 0;
1179

Ulrich Kemloh's avatar
Ulrich Kemloh committed
1180 1181
     for (int i = 0; i < N; i++)
     {
1182 1183 1184 1185 1186 1187 1188
          Line l = Line(_poly[i], _poly[(i + 1) % N]);
          Point s = l.LotPoint(ped);
          if (l.IsInLineSegment(s))
               sum++;
     }
     if (sum == 4)
          rueck = true;
1189

1190
     return rueck;
1191
}
1192 1193 1194 1195 1196


void SubRoom::SetType(const std::string& type)
{
     _type = type;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
1197 1198
}