Obstacle.cpp 10.5 KB
Newer Older
1
/**
Weichen's avatar
Weichen committed
2 3
 * \file        Obstacle.cpp
 * \date        Jul 31, 2012
4 5
 * \version     v0.7
 * \copyright   <2009-2015> 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 27
 **/

28 29

#include "Obstacle.h"
30 31 32 33 34 35

#include <cstdlib>

#include "../IO/OutputHandler.h"
#include "Line.h"
#include "Wall.h"
36

37 38 39 40

using namespace std;


41 42 43 44 45 46 47
Obstacle::Obstacle()
{
     _height=0.0;
     _id=-1;
     _caption="obstacle";
     _walls = vector<Wall > ();
     _poly = vector<Point > ();
48
}
49 50 51 52

Obstacle::~Obstacle() {}


53 54 55
void Obstacle::AddWall(const Wall& w)
{
     _walls.push_back(w);
56 57
}

58 59 60
string Obstacle::GetCaption() const
{
     return _caption;
61 62
}

63 64 65
void Obstacle::SetCaption(string caption)
{
     _caption = caption;
66 67
}

68 69 70
double Obstacle::GetHeight() const
{
     return _height;
71 72
}

73 74 75
void Obstacle::SetHeight(double height)
{
     _height = height;
76 77
}

78 79 80
int Obstacle::GetId() const
{
     return _id;
81 82
}

83 84 85
void Obstacle::SetId(int id)
{
     _id = id;
86 87
}

88 89 90
const vector<Point>& Obstacle::GetPolygon() const
{
     return _poly;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
91 92
}

93 94 95
string Obstacle::Write()
{
     string s;
96

97 98 99 100 101 102
     for (unsigned int j = 0; j < _walls.size(); j++) {
          const Wall& w = _walls[j];
          s.append(w.Write());
          //pos = pos + w.GetPoint1() + w.GetPoint2();
     }
     //pos = pos * (0.5 / _walls.size());
Ulrich Kemloh's avatar
Ulrich Kemloh committed
103

104
     Point pos = GetCentroid();
105

106 107 108 109
     //add the obstacle caption
     char tmp[CLENGTH];
     //sprintf(tmp, "\t\t<label centerX=\"%.2f\" centerY=\"%.2f\" centerZ=\"0\" text=\"%s\" color=\"100\" />\n"
     //              , pos.GetX() * FAKTOR, pos.GetY() * FAKTOR, _caption.c_str());
Ulrich Kemloh's avatar
Ulrich Kemloh committed
110

111
     sprintf(tmp, "\t\t<label centerX=\"%.2f\" centerY=\"%.2f\" centerZ=\"0\" text=\"%d\" color=\"100\" />\n"
Oliver Schmidts's avatar
Oliver Schmidts committed
112
             , pos._x * FAKTOR, pos._y * FAKTOR, _id);
113
     s.append(tmp);
114

115
     return s;
116 117
}

118 119 120
const vector<Wall>& Obstacle::GetAllWalls() const
{
     return _walls;
121 122
}

123 124
int Obstacle::WhichQuad(const Point& vertex, const Point& hitPos) const
{
Oliver Schmidts's avatar
Oliver Schmidts committed
125 126 127
     return (vertex._x > hitPos._x) ?
               ((vertex._y > hitPos._y) ? 1 : 4) :
               ((vertex._y > hitPos._y) ? 2 : 3);
128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148

//     if ((vertex.GetX() - hitPos.GetX())>J_EPS)
//     {
//          if ((vertex.GetY() - hitPos.GetY())>J_EPS)
//          {
//               return 1;
//          } else
//          {
//               return 4;
//          }
//     } else
//     {
//          if ((vertex.GetY() - hitPos.GetY())>J_EPS)
//          {
//               return 2;
//          } else
//          {
//               return 3;
//          }
//
//     }
149 150 151 152

}

// x-Koordinate der Linie von einer Eccke zur nächsten
153 154
double Obstacle::Xintercept(const Point& point1, const Point& point2, double hitY) const
{
Oliver Schmidts's avatar
Oliver Schmidts committed
155 156
     return (point2._x- (((point2._y - hitY) * (point1._x - point2._x)) /
                              (point1._y - point2._y)));
157 158
}

159

160 161
bool Obstacle::Contains(const Point& ped) const
{
162 163 164 165 166 167 168 169
     //case when the point is on an edge
     // todo: this affect the runtime, and do we really need that
     // If we do not d othis check, then for a square for instance, half the points located on the edge will be inside and
     // the other half will be outside the polygon.
     for(auto& w: _walls)
     {
          if(w.IsInLineSegment(ped)) return true;
     }
170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193
     // in the case the obstacle is not a close surface, allow
     // pedestrians distribution 'inside'

     short edge, first, next;
     short quad, next_quad, delta, total;

     /////////////////////////////////////////////////////////////
     edge = first = 0;
     quad = WhichQuad(_poly[edge], ped);
     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
Oliver Schmidts's avatar
Oliver Schmidts committed
194
               if (Xintercept(_poly[edge], _poly[next], ped._y) > ped._x)
195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214
                    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;
          }
          /* 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;
215 216
}

217

218
bool Obstacle::ConvertLineToPoly()
219 220 221 222 223 224
{
     vector<Line*> copy;
     vector<Point> tmpPoly;
     Point point;
     Line* line;
     // Alle Linienelemente in copy speichern
225 226 227 228
     for (auto& w: _walls)
       	 copy.push_back(&w);

     Point pIntsct(J_NAN, J_NAN);
229 230
     int itr = 1;
     for (auto& it : _walls) {
karthik's avatar
karthik committed
231
    	 int j = 0;
232
    	 for (unsigned int i = itr; i < copy.size(); ++i) {
233 234 235 236 237 238 239 240 241 242
    		 if (it.IntersectionWith(*copy[i], pIntsct) == true) {
    			 if (it.ShareCommonPointWith(*copy[i]) == false) {
    				 char tmp[CLENGTH];
    				 sprintf(tmp, "ERROR: \tObstacle::ConvertLineToPoly(): ID %d !!!\n", _id);
    				 Log->Write(tmp);
    				 sprintf(tmp, "ERROR: \tWalls %s & %s intersect: !!!\n", it.toString().c_str(),
    						                                           copy[i]->toString().c_str());
    				 Log->Write(tmp);
    				 return false;
    			 }
karthik's avatar
karthik committed
243 244
    			 else
    				 ++j;
245 246
    		 }
    	 }
247 248 249 250 251 252 253 254 255 256 257 258
         if (j <= 2)
        	 j = 0;
         else {
        	 char tmp[CLENGTH];
        	 sprintf(tmp, "ERROR: \tObstacle::ConvertLineToPoly(): ID %d !!!\n", _id);
        	 Log->Write(tmp);
        	 sprintf(tmp, "ERROR: \tWall %s shares edge with multiple walls!!!\n", it.toString().c_str());
        	 Log->Write(tmp);
        	 return false;
         }
    	 ++itr;
     }
259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283
     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: \tObstacle::ConvertLineToPoly(): ID %d !!!\n", _id);
          Log->Write(tmp);
284 285
          sprintf(tmp, "ERROR: \tDistance between the points: %lf !!!\n", (tmpPoly[0] - point).Norm());
          Log->Write(tmp);
286
          return false;
287 288
     }
     _poly = tmpPoly;
289 290 291 292

     //check if all walls and goals were used in the polygon
     for (const auto& w: _walls)
          for (const auto & ptw: {w.GetPoint1(),w.GetPoint2()})
293
               if(IsPartOfPolygon(ptw)==false) {
294 295 296
                    Log->Write("ERROR:\t Edge was not used during polygon creation for obstacle: %s",w.toString().c_str());
                    return false;
               }
297

298
     return true;
299
}
Ulrich Kemloh's avatar
Ulrich Kemloh committed
300

301 302 303 304 305 306 307 308 309 310 311 312 313 314
const Point Obstacle::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

     // For all vertices except last
     unsigned int i=0;
     for (i=0; i<_poly.size()-1; ++i) {
Oliver Schmidts's avatar
Oliver Schmidts committed
315 316 317 318
          x0 = _poly[i]._x;
          y0 = _poly[i]._y;
          x1 = _poly[i+1]._x;
          y1 = _poly[i+1]._y;
319 320 321 322 323 324 325
          a = x0*y1 - x1*y0;
          signedArea += a;
          px += (x0 + x1)*a;
          py += (y0 + y1)*a;
     }

     // Do last vertex
Oliver Schmidts's avatar
Oliver Schmidts committed
326 327 328 329
     x0 = _poly[i]._x;
     y0 = _poly[i]._y;
     x1 = _poly[0]._x;
     y1 = _poly[0]._y;
330 331 332 333 334 335 336 337 338 339
     a = x0*y1 - x1*y0;
     signedArea += a;
     px += (x0 + x1)*a;
     py += (y0 + y1)*a;

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

     return Point (px,py);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
340
}
341

342 343
bool Obstacle::IsClockwise() const
{
344
     //http://stackoverflow.com/questions/1165647/how-to-determine-if-a-list-of-polygon-points-are-in-clockwise-order
345
     if(_poly.size()<3) {
346
          Log->Write("ERROR:\tYou need at least 3 vertices to check for orientation. Obstacle ID [%d]",_id);
347 348 349
          return false;
          //exit(EXIT_FAILURE);
     }
350
     double sum = 0;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
351
     for (unsigned int i = 0; i < _poly.size() - 1; ++i) {
352 353 354 355 356 357 358 359 360
          Point a = _poly[i];
          Point b = _poly[i+1];
          sum += (b._x - a._x) * (b._y + a._y);
     }
     Point first = _poly[0];
     Point last = _poly[_poly.size()-1];
     sum += (first._x - last._x) * (first._y + last._y);

     return (sum > 0.);
361
}
362

363 364
bool Obstacle::IntersectWithLine(const Line& line) const
{
365

366
     for (unsigned int i=0; i<_walls.size(); i++) {
367

368 369
          if(_walls[i].IntersectionWith(line)) return true;
     }
370

371
     return false;
372
}
373 374 375 376 377 378 379

bool Obstacle::IsPartOfPolygon(const Point& ptw)
{
     if ( false == IsElementInVector(_poly, ptw))
     {
          //maybe the point was too closed to other points and got replaced
          //check that eventuality
380
          bool nah = false;
381 382 383 384
          for (const auto & pt : _poly)
          {
               if ((pt - ptw).Norm() < J_TOLERANZ)
               {
385
                    nah = true;
386 387 388 389
                    break;
               }
          }

390
          if(nah==false)
391 392 393 394 395 396
          {
               return false;
          }
     }
     return true;
}