Goal.cpp 8.94 KB
Newer Older
1
/**
Weichen's avatar
Weichen committed
2 3
 * \file        Goal.cpp
 * \date        Spe 12, 2013
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

Ulrich Kemloh's avatar
Ulrich Kemloh committed
29
#include "Point.h"
30 31
#include "Goal.h"
#include "Wall.h"
32
#include "Crossing.h"
33 34 35 36

using namespace std;


37 38 39 40 41 42 43
Goal::Goal()
{
     _id=-1;
     _caption="Goal";
     _isFinalGoal=0;
     _walls = vector<Wall > ();
     _poly = vector<Point > ();
44
     _crossing = new Crossing();
45 46
     _roomID = -1;
     _subRoomID = -1;
47 48
}

49 50
Goal::~Goal()
{
51 52 53

}

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

59 60 61
string Goal::GetCaption() const
{
     return _caption;
62 63
}

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

69 70 71
int Goal::GetId() const
{
     return _id;
72 73
}

74 75 76
void Goal::SetId(int id)
{
     _id = id;
77
     _crossing->SetID(id);
78 79
}

80 81 82
const vector<Point>& Goal::GetPolygon() const
{
     return _poly;
83 84
}

85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104
string Goal::Write()
{
     string s;
     Point pos;

     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());

     // add some fancy stuffs
     if(_poly.size()>=4) {
          s.append(Wall(_poly[0],_poly[2]).Write());
          s.append(Wall(_poly[1],_poly[3]).Write());
     }
     //add the Goal caption
     char tmp[CLENGTH];
     sprintf(tmp, "\t\t<label centerX=\"%.2f\" centerY=\"%.2f\" centerZ=\"0\" text=\"%s\" color=\"100\" />\n"
Oliver Schmidts's avatar
Oliver Schmidts committed
105
             , pos._x * FAKTOR, pos._y * FAKTOR, _caption.c_str());
106 107 108
     s.append(tmp);

     return s;
109 110
}

111 112 113
const vector<Wall>& Goal::GetAllWalls() const
{
     return _walls;
114 115
}

116 117
int Goal::WhichQuad(const Point& vertex, const Point& hitPos) const
{
Oliver Schmidts's avatar
Oliver Schmidts committed
118 119
     return (vertex._x > hitPos._x) ? ((vertex._y > hitPos._y) ? 1 : 4) :
                 ((vertex._y > hitPos._y) ? 2 : 3);
120 121 122

}

123 124 125
int Goal::GetIsFinalGoal() const
{
     return _isFinalGoal;
126 127
}

128 129 130
void Goal::SetIsFinalGoal(int isFinalGoal)
{
     _isFinalGoal = isFinalGoal;
131 132 133
}

// x-Koordinate der Linie von einer Eccke zur nächsten
134 135
double Goal::Xintercept(const Point& point1, const Point& point2, double hitY) const
{
Oliver Schmidts's avatar
Oliver Schmidts committed
136 137
     return (point2._x - (((point2._y - hitY) * (point1._x - point2._x)) /
                              (point1._y - point2._y)));
138 139 140
}


141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165
bool Goal::Contains(const Point& ped) const
{


     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
166
               if (Xintercept(_poly[edge], _poly[next], ped._y) > ped._x)
167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186
                    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;
187 188
}

189
bool Goal::ConvertLineToPoly()
190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224
{
     vector<Line*> copy;
     vector<Point> tmpPoly;
     Point point;
     Line* line;
     // Alle Linienelemente in copy speichern
     for (unsigned int i = 0; i < _walls.size(); i++) {
          copy.push_back(&_walls[i]);
     }

     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: \tGoal::ConvertLineToPoly(): ID %d !!!\n", _id);
          Log->Write(tmp);
225
          return false;
226 227
     }
     _poly = tmpPoly;
228 229 230 231 232 233 234 235 236 237 238 239 240
     CreateBoostPoly();
     ComputeCentroid();

     //compute dummy crossing in the middle
     Point point1, point2, tmp, diff, diff1, diff2;
     if (_poly.size() %2 == 0){
          point1 = _poly[0];
          tmp = _poly[_poly.size()/2];
          diff = point1 - tmp;

          point1 = tmp +  diff * 0.95;
          point2 = tmp +  diff * 0.05;

241 242
          _crossing->SetPoint1(point1);
          _crossing->SetPoint2(point2);
243
     }else{
244
          _crossing->SetPoint1(_poly[0]);
Ozaq's avatar
Ozaq committed
245 246
          Line tmp_line(_poly[_poly.size()/2], _poly[(_poly.size()/2)+1], 0);
          _crossing->SetPoint2(tmp_line.GetCentre());
247 248 249
     }


250
//     std::cout << "Crossing goal: " << _crossing->GetUniqueID() << _crossing->toString() << std::endl;
251
     return true;
252 253
}

254 255 256
const Point& Goal::GetCentroid() const
{
     return _centroid;
257 258
}

259
void  Goal::ComputeCentroid()
260 261 262 263 264 265 266 267 268 269 270 271 272
{

     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
273 274 275 276
          x0 = _poly[i]._x;
          y0 = _poly[i]._y;
          x1 = _poly[i+1]._x;
          y1 = _poly[i+1]._y;
277 278 279 280 281 282 283
          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
284 285 286 287
     x0 = _poly[i]._x;
     y0 = _poly[i]._y;
     x1 = _poly[0]._x;
     y1 = _poly[0]._y;
288 289 290 291 292 293 294 295 296 297 298
     a = x0*y1 - x1*y0;
     signedArea += a;
     px += (x0 + x1)*a;
     py += (y0 + y1)*a;

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

     _centroid._x=px;
     _centroid._y=py;
299
}
300

301
Crossing* Goal::GetCentreCrossing()
302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348
{
     return _crossing;
}

//bool Goal::IsInsideGoal(Pedestrian* ped) const
//{
//     return IsInsideGoal(ped->GetPos());
//}

bool Goal::IsInsideGoal(const Point& point) const
{
     return boost::geometry::within(point, _boostPoly);
}

bool Goal::CreateBoostPoly() {
     std::vector<Point> copyPts;
     copyPts.insert(copyPts.begin(), _poly.begin(), _poly.end());

     if(!IsClockwise()){
          std::reverse(copyPts.begin(), copyPts.end());
     }

     boost::geometry::assign_points(_boostPoly, _poly);

     return true;
}

bool Goal::IsClockwise()
{
     //http://stackoverflow.com/questions/1165647/how-to-determine-if-a-list-of-polygon-points-are-in-clockwise-order
     if(_poly.size()<3) {
          Log->Write("ERROR:\tYou need at least 3 vertices to check for orientation. Obstacle ID [%d]",_id);
          return false;
          //exit(EXIT_FAILURE);
     }
     double sum = 0;
     for (unsigned int i = 0; i < _poly.size() - 1; ++i) {
          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.);
}
349 350 351 352 353 354

int Goal::GetRoomID() const
{
     return _roomID;
}

Ozaq's avatar
Ozaq committed
355
void Goal::SetRoomID(int roomID)
356
{
Ozaq's avatar
Ozaq committed
357
     _roomID = roomID;
358 359 360 361 362 363 364
}

int Goal::GetSubRoomID() const
{
     return _subRoomID;
}

Ozaq's avatar
Ozaq committed
365
void Goal::SetSubRoomID(int subRoomID)
366
{
Ozaq's avatar
Ozaq committed
367
     _subRoomID = subRoomID;
368
}