44 #ifdef CHECK_MEMORY_LEAKS
46 #endif // CHECK_MEMORY_LEAKS
58 const SUMOReal eps = std::numeric_limits<SUMOReal>::epsilon();
59 const double denominator = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
60 const double numera = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3);
61 const double numerb = (x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3);
63 if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
70 a1 = x1 < x2 ? x1 : x2;
71 a2 = x1 < x2 ? x2 : x1;
72 a3 = x3 < x4 ? x3 : x4;
73 a4 = x3 < x4 ? x4 : x3;
75 a1 = y1 < y2 ? y1 : y2;
76 a2 = y1 < y2 ? y2 : y1;
77 a3 = y3 < y4 ? y3 : y4;
78 a4 = y3 < y4 ? y4 : y3;
80 if (a1 <= a3 && a3 <= a2) {
87 if (a3 <= a1 && a1 <= a4) {
97 *mu = (a - x1) / (x2 - x1);
99 *y = y1 + (*mu) * (y2 - y1);
106 *mu = (a - y1) / (y2 - y1);
115 if (fabs(denominator) < eps) {
119 const double mua = numera / denominator;
120 const double mub = numerb / denominator;
121 if (mua < 0 || mua > 1 || mub < 0 || mub > 1) {
125 *x = x1 + mua * (x2 - x1);
126 *y = y1 + mua * (y2 - y1);
138 p21.
x(), p21.
y(), p22.
x(), p22.
y(), 0, 0, 0);
144 if (p.
x() >=
MIN2(from.
x(), to.
x()) && p.
x() <=
MAX2(from.
x(), to.
x()) &&
145 p.
y() >=
MIN2(from.
y(), to.
y()) && p.
y() <=
MAX2(from.
y(), to.
y())) {
154 std::vector<SUMOReal>& into) {
159 SUMOReal B = 2 * (dx * (p1.
x() - c.
x()) + dy * (p1.
y() - c.
y()));
160 SUMOReal C = (p1.
x() - c.
x()) * (p1.
x() - c.
x()) + (p1.
y() - c.
y()) * (p1.
y() - c.
y()) - radius * radius;
163 if ((A <= 0.0000001) || (det < 0)) {
166 }
else if (det == 0) {
169 Position intersection(p1.
x() + t * dx, p1.
y() + t * dy);
177 Position intersection(p1.
x() + t * dx, p1.
y() + t * dy);
181 t = (
float)((-B - sqrt(det)) / (2 * A));
182 intersection.
set(p1.
x() + t * dx, p1.
y() + t * dy);
199 p21.
x(), p21.
y(), p22.
x(), p22.
y(), &x, &y, &m)) {
201 return Position(x, y, p11.
z() + m * (p12.
z() - p11.
z()));
215 SUMOReal dtheta = atan2(y2, x2) - atan2(y1, x1);
230 return p1 + (p2 - p1) * factor;
238 return p1 - (p2 - p1) * factor;
246 return p2 - (p1 - p2) * factor;
253 const Position& Point,
bool perpendicular) {
255 if (lineLength2D == 0.0f) {
260 const SUMOReal u = (((Point.
x() - LineStart.
x()) * (LineEnd.
x() - LineStart.
x())) +
261 ((Point.
y() - LineStart.
y()) * (LineEnd.
y() - LineStart.
y()))
262 ) / (lineLength2D * lineLength2D);
263 if (u < 0.0f || u > 1.0f) {
272 return u * lineLength2D;
282 if (lineLengthSquared == 0.0f) {
287 SUMOReal u = (((point.
x() - lineStart.
x()) * (lineEnd.
x() - lineStart.
x())) +
288 ((point.
y() - lineStart.
y()) * (lineEnd.
y() - lineStart.
y()))
289 ) / lineLengthSquared;
290 if (u < 0.0f || u > 1.0f) {
294 lineStart.
x() + u * (lineEnd.
x() - lineStart.
x()),
295 lineStart.
y() + u * (lineEnd.
y() - lineStart.
y()));
307 outIntersection.
set(
Line(lineStart, lineEnd).getPositionAtDistance(length));
318 const SUMOReal dx = lineBeg.
x() - lineEnd.
x();
319 const SUMOReal dy = lineBeg.
y() - lineEnd.
y();
320 const SUMOReal length = sqrt(dx * dx + dy * dy);
322 p.
add(dy * amount / length, -dx * amount / length);
354 std::pair<SUMOReal, SUMOReal>
362 std::pair<SUMOReal, SUMOReal>
369 return std::pair<SUMOReal, SUMOReal>
370 ((beg.
y() - end.
y()) * wanted_offset / length, (end.
x() - beg.
x()) * wanted_offset / length);
static std::pair< SUMOReal, SUMOReal > getNormal90D_CW(const Position &beg, const Position &end, SUMOReal length, SUMOReal wanted_offset)
static SUMOReal Angle2D(SUMOReal x1, SUMOReal y1, SUMOReal x2, SUMOReal y2)
static SUMOReal getCWAngleDiff(SUMOReal angle1, SUMOReal angle2)
Returns the distance of second angle from first angle clockwise.
static SUMOReal getCCWAngleDiff(SUMOReal angle1, SUMOReal angle2)
Returns the distance of second angle from first angle counter-clockwise.
static Position intersection_position2D(const Position &p11, const Position &p12, const Position &p21, const Position &p22)
returns the intersection point of the (infinite) lines p11,p12 and p21,p22. If the given lines are pa...
static Position interpolate(const Position &p1, const Position &p2, SUMOReal length)
void add(const Position &pos)
Adds the given position to this one.
SUMOReal distanceSquaredTo(const Position &p2) const
SUMOReal ymin() const
Returns minimum y-coordinate.
SUMOReal xmin() const
Returns minimum x-coordinate.
bool intersects(const Position &p1, const Position &p2) const
static Position extrapolate_second(const Position &p1, const Position &p2, SUMOReal length)
SUMOReal distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
static Position extrapolate_first(const Position &p1, const Position &p2, SUMOReal length)
static void FindLineCircleIntersections(const Position &c, SUMOReal radius, const Position &p1, const Position &p2, std::vector< SUMOReal > &into)
Returns the positions the given circle is crossed by the given line.
static Position crossPoint(const Boundary &b, const PositionVector &v)
SUMOReal x() const
Returns the x-position.
SUMOReal xmax() const
Returns maximum x-coordinate.
A class that stores a 2D geometrical boundary.
static SUMOReal nearest_offset_on_line_to_point2D(const Position &l1, const Position &l2, const Position &p, bool perpendicular=true)
static bool intersects(const Position &p11, const Position &p12, const Position &p21, const Position &p22)
return whether given lines intersect
A point in 2D or 3D with translation and scaling methods.
static Position transfer_to_side(Position &p, const Position &lineBeg, const Position &lineEnd, SUMOReal amount)
static SUMOReal distancePointLine(const Position &point, const Position &lineStart, const Position &lineEnd)
SUMOReal z() const
Returns the z-position.
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
Position intersectsAtPoint(const Position &p1, const Position &p2) const
SUMOReal y() const
Returns the y-position.
static SUMOReal getMinAngleDiff(SUMOReal angle1, SUMOReal angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
void set(SUMOReal x, SUMOReal y)
static SUMOReal getMaxAngleDiff(SUMOReal angle1, SUMOReal angle2)
Returns the maximum distance (clockwise/counter-clockwise) between both angles.
SUMOReal distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
SUMOReal ymax() const
Returns maximum y-coordinate.
static bool pointOnLine(const Position &p, const Position &from, const Position &to)
Returns whether the given point lies on the given line.
static SUMOReal closestDistancePointLine(const Position &point, const Position &lineStart, const Position &lineEnd, Position &outIntersection)