Line.cpp 15.1 KB
Newer Older
1
/**
Weichen's avatar
Weichen committed
2 3
 * \file        Line.cpp
 * \date        Sep 30, 2010
4 5
 * \version     v0.7
 * \copyright   <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
Ulrich Kemloh's avatar
Ulrich Kemloh committed
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
 **/

Ulrich Kemloh's avatar
Ulrich Kemloh committed
28

29
#include "Point.h"
30
//#include "SubRoom.h"
31
#include "../general/Macros.h"
Ulrich Kemloh's avatar
Ulrich Kemloh committed
32
#include "Line.h"
Ulrich Kemloh's avatar
Ulrich Kemloh committed
33
#include "../IO/OutputHandler.h"
Ulrich Kemloh's avatar
Ulrich Kemloh committed
34

35 36 37

#include  <cmath>
#include  <sstream>
38

39
int Line::_static_UID = 0;
40

41
using namespace std;
42

Ulrich Kemloh's avatar
Ulrich Kemloh committed
43 44 45
/************************************************************
  Konstruktoren
 ************************************************************/
46 47 48 49
Line::Line() {
    SetPoint1(Point()); //Default-Constructor  (0.0,0.0)
    SetPoint2(Point());
    _uid = _static_UID++;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
50 51
}

52 53 54 55
Line::Line(const Point &p1, const Point &p2) {
    SetPoint1(p1);
    SetPoint2(p2);
    _uid = _static_UID++;
56 57
}

58 59
int Line::GetUniqueID() const {
    return _uid;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
60 61
}

62 63 64 65 66
Line::Line(const Line &orig) {
    _point1 = orig.GetPoint1();
    _point2 = orig.GetPoint2();
    _centre = orig.GetCentre();
    _uid = orig.GetUniqueID();
Ulrich Kemloh's avatar
Ulrich Kemloh committed
67 68
}

69
Line::~Line() {
70
}
Ulrich Kemloh's avatar
Ulrich Kemloh committed
71 72 73 74

/*************************************************************
 Setter-Funktionen
 ************************************************************/
75 76 77
void Line::SetPoint1(const Point &p) {
    _point1 = p;
    _centre = (_point1 + _point2) * 0.5;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
78 79
}

80 81 82
void Line::SetPoint2(const Point &p) {
    _point2 = p;
    _centre = (_point1 + _point2) * 0.5;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
83 84 85 86 87
}

/*************************************************************
 Getter-Funktionen
 ************************************************************/
88 89
const Point &Line::GetPoint1() const {
    return _point1;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
90 91
}

92 93
const Point &Line::GetPoint2() const {
    return _point2;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
94 95
}

96 97
const Point &Line::GetCentre() const {
    return _centre;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
98 99 100 101 102
}

/*************************************************************
 Ausgabe
 ************************************************************/
103 104 105 106 107 108 109 110 111 112 113 114 115 116
string Line::Write() const {
    string geometry;
    char wall[500] = "";
    geometry.append("\t\t<wall color=\"100\">\n");
    sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",
            (GetPoint1().GetX()) * FAKTOR,
            (GetPoint1().GetY()) * FAKTOR);
    geometry.append(wall);
    sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",
            (GetPoint2().GetX()) * FAKTOR,
            (GetPoint2().GetY()) * FAKTOR);
    geometry.append(wall);
    geometry.append("\t\t</wall>\n");
    return geometry;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
117 118 119 120 121 122 123 124
}


/*************************************************************
 Sonstige Funktionen
 ************************************************************/
// Normalen vector zur Linie

125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144
Point Line::NormalVec() const {
    double nx, ny, norm;
    Point r = GetPoint2() - GetPoint1();

    if (r.GetX() == 0.0) {
        nx = 1;
        ny = 0;
    } else {
        nx = -r.GetY() / r.GetX();
        ny = 1;
        /* Normieren */
        norm = sqrt(nx * nx + ny * ny);
        if (fabs(norm) < J_EPS) {
            Log->Write("ERROR: \tLine::NormalVec() norm==0\n");
            exit(0);
        }
        nx /= norm;
        ny /= norm;
    }
    return Point(nx, ny);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
145 146 147 148
}

// Normale Komponente von v auf l

149 150 151 152
double Line::NormalComp(const Point &v) const {
    // Normierte Vectoren
    Point l = (GetPoint2() - GetPoint1()).Normalized();
    const Point &n = NormalVec();
Ulrich Kemloh's avatar
Ulrich Kemloh committed
153 154


155 156 157 158 159
    double lx = l.GetX();
    double ly = l.GetY();
    double nx = n.GetX();
    double ny = n.GetY();
    double alpha;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
160

161 162 163 164 165 166 167
    if (fabs(lx) < J_EPS) {
        alpha = v.GetX() / nx;
    } else if (fabs(ly) < J_EPS) {
        alpha = v.GetY() / ny;
    } else {
        alpha = (v.GetY() * lx - v.GetX() * ly) / (nx * ly - ny * lx);
    }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
168

169
    return fabs(alpha);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
170 171 172 173
}
// Lotfußpunkt zur Geraden Line
// Muss nicht im Segment liegen

174 175 176 177 178 179
Point Line::LotPoint(const Point &p) const {
    const Point &r = GetPoint1();
    const Point &s = GetPoint2();
    const Point &t = r - s;
    Point tmp;
    double lambda;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
180

181 182 183 184
    tmp = p - s;
    lambda = tmp.ScalarProduct(t) / t.ScalarProduct(t);
    Point f = s + t * lambda;
    return f;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
185 186 187 188 189 190
}

/* Punkt auf der Linie mit kürzestem Abstand zu p
 * In der Regel Lotfußpunkt, Ist der Lotfußpunkt nicht im Segment
 * wird der entsprechende Eckpunkt der Line genommen
 * */
191
Point Line::ShortestPoint(const Point &p) const {
Ulrich Kemloh's avatar
Ulrich Kemloh committed
192

193 194 195 196 197 198
    const Point &t = _point1 - _point2;
    if (_point1 == _point2)
        return _point1;
    Point tmp = p - _point2;
    double lambda = tmp.ScalarProduct(t) / t.ScalarProduct(t);
    Point f = _point2 + t * lambda;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
199

200 201 202 203 204
    /* Prüfen ob Punkt in der Linie,sonst entsprechenden Eckpunkt zurückgeben */
    if (lambda < 0)
        f = _point2;
    if (lambda > 1)
        f = _point1;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
205

206
    return f;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
207 208
}

209 210 211 212
/*
 *  Prüft, ob Punkt p im Liniensegment enthalten ist
 * algorithm from:
 * http://stackoverflow.com/questions/328107/how-can-you-determine-a-point-is-between-two-other-points-on-a-line-segment
Ulrich Kemloh's avatar
Ulrich Kemloh committed
213
 *
214
 * */
215 216
bool Line::IsInLineSegment(const Point &p) const
{
Ulrich Kemloh's avatar
Ulrich Kemloh committed
217
     return fabs( (_point1-p ).Norm() + (_point2-p ).Norm() - (_point2-_point1 ).Norm() )<J_EPS;
218 219
}

Ulrich Kemloh's avatar
Ulrich Kemloh committed
220 221 222 223
/* Berechnet direkt den Abstand von p zum Segment l
 * dazu wird die Funktion Line::ShortestPoint()
 * benuzt
 * */
224 225
double Line::DistTo(const Point &p) const {
    return (p - ShortestPoint(p)).Norm();
Ulrich Kemloh's avatar
Ulrich Kemloh committed
226 227
}

228 229
double Line::DistToSquare(const Point &p) const {
    return DistTo(p) * DistTo(p);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
230 231
}

232
// bool Line::operator*(const Line& l) const {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
233 234
//      return ((_point1*l.GetPoint1() && _point2 == l.GetPoint2()) ||
//                      (_point2 == l.GetPoint1() && _point1 == l.GetPoint2()));
235 236 237
// }


Ulrich Kemloh's avatar
Ulrich Kemloh committed
238 239 240
/* Zwei Linien sind gleich, wenn ihre beiden Punkte
 * gleich sind
 * */
241 242 243
bool Line::operator==(const Line &l) const {
    return ((_point1 == l.GetPoint1() && _point2 == l.GetPoint2()) ||
            (_point2 == l.GetPoint1() && _point1 == l.GetPoint2()));
Ulrich Kemloh's avatar
Ulrich Kemloh committed
244 245 246
}

/* Zwei Linien sind ungleich, wenn ihre beiden Punkte
247
 * ungleich sind.
Ulrich Kemloh's avatar
Ulrich Kemloh committed
248
 * */
249 250
bool Line::operator!=(const Line &l) const {
    return (!(*this==l));
251

Ulrich Kemloh's avatar
Ulrich Kemloh committed
252 253
}

254 255
double Line::Length() const {
    return (_point1 - _point2).Norm();
Ulrich Kemloh's avatar
Ulrich Kemloh committed
256 257
}

258 259
double Line::LengthSquare() const {
    return (_point1 - _point2).NormSquare();
Ulrich Kemloh's avatar
Ulrich Kemloh committed
260 261
}

Ulrich Kemloh's avatar
Ulrich Kemloh committed
262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285
//TODO unit  test
bool Line::Overlapp(const Line& l) const
{
     //first check if they are colinear
     Point vecAB=l.GetPoint2()-l.GetPoint1();
     Point vecDC=_point1-_point2;
     if(fabs(vecAB.Determinant(vecDC))<J_EPS)
     {

          if( IsInLineSegment(l.GetPoint1()) and  not  HasEndPoint(l.GetPoint1()))
          {
               //Log->Write("ERROR: 1. Overlapping walls %s and %s ", toString().c_str(),l.toString().c_str());
               return true;
          }

          if( IsInLineSegment(l.GetPoint2()) and not HasEndPoint(l.GetPoint2()))
          {
               //Log->Write("ERROR: 2. Overlapping walls %s and %s ", toString().c_str(),l.toString().c_str());
               return true;
          }
     }
     return false;
}

286 287 288 289 290 291 292 293
//FIXME no equals check with == on double or float bring in an epsilon
bool Line::IntersectionWith(const Point &p1, const Point &p2) const {
    Point AC = _point1 - p1;
    Point DC = p2 - p1;
    Point BA = _point2 - _point1;
    double denominator = BA.CrossProduct(DC);
    double numerator = DC.CrossProduct(AC);

294
    if (denominator == 0.0) {
295

296 297
        // the lines are superposed
        if (numerator == 0.0) {
298

299
            // the segment are superposed
300 301
            return IsInLineSegment(p1) || IsInLineSegment(p2);

302
        } else { // the lines are just parallel and do not share a common point
303

304 305 306
            return false;
        }
    }
307

308 309 310 311 312 313 314 315 316 317
    double r = numerator / denominator;
    if (r < 0.0 || r > 1.0) {
        return false;
    }

    double s = (BA.CrossProduct(AC)) / denominator;
    if (s < 0.0 || s > 1.0) {
        return false;
    }

318
    return true;
319

320
}
Ulrich Kemloh's avatar
Ulrich Kemloh committed
321

322
bool Line::IntersectionWith(const Line &l) const {
323
    return IntersectionWith(l._point1, l._point2);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
324 325
}

326
Line Line::Enlarge(double d) const {
327 328 329
    const Point &p1 = _point1;
    const Point &p2 = _point2;
    Point diff = (p1 - p2).Normalized() * d;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
330

331
    return Line(p1 + diff, p2 - diff);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
332 333
}

334 335
bool Line::IsHorizontal() {
    return fabs(_point1._y - _point2._y) <= J_EPS;
336 337
}

338 339
bool Line::IsVertical() {
    return fabs(_point1._x - _point2._x) <= J_EPS;
340 341
}

342
int Line::WichSide(const Point &pt) {
343

344 345
     if(IsLeft(pt)) return 0;
     return 1;
346 347
}

Ulrich Kemloh's avatar
Ulrich Kemloh committed
348

349 350 351
bool Line::ShareCommonPointWith(const Line &line) const {
    if (line.GetPoint1() == _point1) return true;
    if (line.GetPoint2() == _point1) return true;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
352

353 354
    if (line.GetPoint1() == _point2) return true;
    return line.GetPoint2() == _point2;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
355 356 357

}

358 359 360
bool Line::HasEndPoint(const Point &point) const {
    if (_point1 == point) return true;
    return _point2 == point;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
361 362
}

363
bool Line::IntersectionWithCircle(const Point &centre, double radius /*cm for pedestrians*/) {
Ulrich Kemloh's avatar
Ulrich Kemloh committed
364

365 366 367
    double r = radius;
    double x1 = _point1.GetX();
    double y1 = _point1.GetY();
Ulrich Kemloh's avatar
Ulrich Kemloh committed
368

369 370
    double x2 = _point2.GetX();
    double y2 = _point2.GetY();
Ulrich Kemloh's avatar
Ulrich Kemloh committed
371

372 373
    double xc = centre.GetX();
    double yc = centre.GetY();
Ulrich Kemloh's avatar
Ulrich Kemloh committed
374

375 376 377 378 379 380 381
    //this formula assumes that the circle is centered the origin.
    // so we translate the complete stuff such that the circle ends up at the origin
    x1 = x1 - xc;
    y1 = y1 - yc;
    x2 = x2 - xc;
    y2 = y2 - yc;
    //xc=xc-xc;yc=yc-yc; to make it perfect
Ulrich Kemloh's avatar
Ulrich Kemloh committed
382

383 384 385 386
    // we first check the intersection of the circle and the  infinite line defined by the segment
    double dr2 = ((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
    double D2 = (x1 * y2 - x2 * y1) * (x1 * y2 - x2 * y1);
    double r2 = radius * radius;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
387

388 389
    double delta = r2 * dr2 - D2;
    if (delta <= 0.0) return false;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
390 391


392 393 394
    double a = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
    double b = 2 * ((x1 * (x2 - x1)) + y1 * (y2 - y1));
    double c = x1 * x1 + y1 * y1 - r * r;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
395

396
    delta = b * b - 4 * a * c;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
397

398 399 400 401 402 403 404 405 406 407 408 409
    if ((x1 == x2) && (y1 == y2)) {
        Log->Write("isLineCrossingCircle: Your line is a point");
        return false;
    }
    if (delta < 0.0) {
        char tmp[CLENGTH];
        sprintf(tmp, "there is a bug in 'isLineCrossingCircle', delta(%f) can t be <0 at this point.", delta);
        Log->Write(tmp);
        Log->Write("press ENTER");
        return false; //fixme
        //getc(stdin);
    }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
410

411 412 413 414 415
    double t1 = (-b + sqrt(delta)) / (2 * a);
    double t2 = (-b - sqrt(delta)) / (2 * a);
    if ((t1 < 0.0) || (t1 > 1.0)) return false;
    if ((t2 < 0.0) || (t2 > 1.0)) return false;
    return true;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
416
}
417

418
//TODO: Consider numerical stability and special case pt is on line
Dominik's avatar
Dominik committed
419
// Returns true if pt is on the left side ( from point1 toward point2)
420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439
bool Line::IsLeft(const Point &pt) {
    double test =
            (_point2._x - _point1._x) * (pt.GetY() - _point1._y) - (_point2._y - _point1._y) * (pt.GetX() - _point1._x);
    return test > 0.0;
}

const Point &Line::GetLeft(const Point &pt) {
    if (IsLeft(pt)) {
        return _point2;
    } else {
        return _point1;
    }
}

const Point &Line::GetRight(const Point &pt) {
    if (!IsLeft(pt)) {
        return _point2;
    } else {
        return _point1;
    }
440
}
441 442 443 444 445 446 447

std::string Line::toString() const {
    std::stringstream tmp;
    tmp << _point1.toString() << "--" << _point2.toString();
    return tmp.str();
}

448 449 450 451
// get distance between first point of line with the intersection point.
//if no intersection return infinity
// this function is exactly the same as GetIntersection(), but returns the distance squared
//insteed of a boolian
452
double Line::GetIntersectionDistance(const Line &l) const {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
453
#define DEBUG 0
454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496
    double deltaACy = _point1.GetY() - l.GetPoint1().GetY();
    double deltaDCx = l.GetPoint2().GetX() - l.GetPoint1().GetX();
    double deltaACx = _point1.GetX() - l.GetPoint1().GetX();
    double deltaDCy = l.GetPoint2().GetY() - l.GetPoint1().GetY();
    double deltaBAx = _point2.GetX() - _point1.GetX();
    double deltaBAy = _point2.GetY() - _point1.GetY();

    double denominator = deltaBAx * deltaDCy - deltaBAy * deltaDCx;
    double numerator = deltaACy * deltaDCx - deltaACx * deltaDCy;
    double infinity = 100000;
    // the lines are parallel
    if (denominator == 0.0) {

        // the lines are superposed
        if (numerator == 0.0) {

            // the segment are superposed
            if (IsInLineSegment(l.GetPoint1()) ||
                IsInLineSegment(l.GetPoint2()))
                return infinity;//really?
            else return infinity;

        } else { // the lines are just parallel and do not share a common point

            return infinity;
        }
    }

    // the lines intersect
    double r = numerator / denominator;
    if (r < 0.0 || r > 1.0) {
        return infinity;
    }

    double s = (deltaACy * deltaBAx - deltaACx * deltaBAy) / denominator;
    if (s < 0.0 || s > 1.0) {
        return infinity;
    }

    Point PointF = Point((float) (_point1._x + r * deltaBAx), (float) (_point1._y + r * deltaBAy));
    if (!IsInLineSegment(PointF)) //is point on the line?
        return infinity;
    double dist = (_point1 - PointF).NormSquare();
Mohcine Chraibi's avatar
Mohcine Chraibi committed
497 498 499 500 501 502 503
#if DEBUG
     printf("Enter GetIntersection\n");
     cout<< "\t" << l.toString() << " intersects with " << toString() <<endl;
     cout<<"\t at point " << PointF.toString()<<endl;
     cout <<  "\t\t --> distance is "<< sqrt(dist)<< "... return "<< dist<<endl;
     printf("Leave GetIntersection\n");
#endif
504
    return dist;
505 506

}
Mohcine Chraibi's avatar
Mohcine Chraibi committed
507 508 509

// calculates the angles QPF and QPL 
// return the snagle of the point (F or L) which is nearer to the Goal 
Mohcine Chraibi's avatar
Mohcine Chraibi committed
510 511 512 513 514 515 516 517 518 519 520
//the calling line: P->Q, Q is the crossing point
// 
//                 o P 
//                / 
//               /  
//   F          /              L 
//   o --------x---------------o  
//            / Q
//           /
//          o Goal

521 522
double Line::GetDeviationAngle(const Line &l) const {
    // const double PI = 3.14159258;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
523
#define DEBUG 0
524 525 526 527 528 529 530 531 532 533 534 535 536 537 538
    Point P = _point1;
    Point Goal = _point2;

    Point L = l._point1;
    Point R = l._point2;

    double dist_Goal_L = (Goal - L).NormSquare();
    double dist_Goal_R = (Goal - R).NormSquare();

    double angle, angleL, angleR;
    // we don't need to calculate both angles, but for debugging purposes we do it.
    angleL = atan((Goal - P).CrossProduct(L - P) / (Goal - P).ScalarProduct(L - P));
    angleR = atan((Goal - P).CrossProduct(R - P) / (Goal - P).ScalarProduct(R - P));

    angle = (dist_Goal_L < dist_Goal_R) ? angleL : angleR;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
539 540 541 542 543 544 545 546 547 548 549 550 551
#if DEBUG
     printf("Enter GetAngel()\n");
     printf("\tP=[%f,%f]\n",P.GetX(), P.GetY());
     printf("\tGoal=[%f,%f]\n",Goal.GetX(), Goal.GetY());
     printf("\tL=[%f,%f]\n",L.GetX(), L.GetY());
     printf("\tR=[%f,%f]\n",R.GetX(), R.GetY());
     printf("\t\tdist_Goal_L=%f\n",dist_Goal_L);
     printf("\t\tdist_Goal_R=%f\n",dist_Goal_R);
     printf("\t\t --> angleL=%f\n",angleL);
     printf("\t\t --> angleR=%f\n",angleR);
     printf("\t\t --> angle=%f\n",angle);
     printf("Leave GetAngel()\n");
#endif
552
    return angle;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
553
}
554 555 556 557 558 559 560 561 562









563

564

565 566 567



568