44 #ifdef CHECK_MEMORY_LEAKS
46 #endif // CHECK_MEMORY_LEAKS
63 const SUMOReal eps = std::numeric_limits<SUMOReal>::epsilon();
64 const double denominator = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
65 const double numera = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3);
66 const double numerb = (x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3);
68 if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
75 a1 = x1 < x2 ? x1 : x2;
76 a2 = x1 < x2 ? x2 : x1;
77 a3 = x3 < x4 ? x3 : x4;
78 a4 = x3 < x4 ? x4 : x3;
80 a1 = y1 < y2 ? y1 : y2;
81 a2 = y1 < y2 ? y2 : y1;
82 a3 = y3 < y4 ? y3 : y4;
83 a4 = y3 < y4 ? y4 : y3;
85 if (a1 <= a3 && a3 <= a2) {
92 if (a3 <= a1 && a1 <= a4) {
102 *mu = (a - x1) / (x2 - x1);
104 *y = y1 + (*mu) * (y2 - y1);
111 *mu = (a - y1) / (y2 - y1);
120 if (fabs(denominator) < eps) {
124 double mua = numera / denominator;
126 if (fabs(x2 - x4) < eps && fabs(y2 - y4) < eps) {
129 const double mub = numerb / denominator;
130 if (mua < 0 || mua > 1 || mub < 0 || mub > 1) {
135 *x = x1 + mua * (x2 - x1);
136 *y = y1 + mua * (y2 - y1);
148 p21.
x(), p21.
y(), p22.
x(), p22.
y(), 0, 0, 0);
154 if (p.
x() >=
MIN2(from.
x(), to.
x()) && p.
x() <=
MAX2(from.
x(), to.
x()) &&
155 p.
y() >=
MIN2(from.
y(), to.
y()) && p.
y() <=
MAX2(from.
y(), to.
y())) {
164 std::vector<SUMOReal>& into) {
169 SUMOReal B = 2 * (dx * (p1.
x() - c.
x()) + dy * (p1.
y() - c.
y()));
170 SUMOReal C = (p1.
x() - c.
x()) * (p1.
x() - c.
x()) + (p1.
y() - c.
y()) * (p1.
y() - c.
y()) - radius * radius;
173 if ((A <= 0.0000001) || (det < 0)) {
176 }
else if (det == 0) {
179 Position intersection(p1.
x() + t * dx, p1.
y() + t * dy);
186 SUMOReal t = (float)((-B + sqrt(det)) / (2 * A));
187 Position intersection(p1.
x() + t * dx, p1.
y() + t * dy);
191 t = (float)((-B - sqrt(det)) / (2 * A));
192 intersection.
set(p1.
x() + t * dx, p1.
y() + t * dy);
209 p21.
x(), p21.
y(), p22.
x(), p22.
y(), &x, &y, &m)) {
211 return Position(x, y, p11.
z() + m * (p12.
z() - p11.
z()));
225 SUMOReal dtheta = atan2(y2, x2) - atan2(y1, x1);
240 return p1 + (p2 - p1) * factor;
248 return p1 - (p2 - p1) * factor;
256 return p2 - (p1 - p2) * factor;
263 const Position& Point,
bool perpendicular) {
265 if (lineLength2D == 0.0f) {
270 const SUMOReal u = (((Point.
x() - LineStart.
x()) * (LineEnd.
x() - LineStart.
x())) +
271 ((Point.
y() - LineStart.
y()) * (LineEnd.
y() - LineStart.
y()))
272 ) / (lineLength2D * lineLength2D);
273 if (u < 0.0f || u > 1.0f) {
282 return u * lineLength2D;
292 if (lineLengthSquared == 0.0f) {
297 SUMOReal u = (((point.
x() - lineStart.
x()) * (lineEnd.
x() - lineStart.
x())) +
298 ((point.
y() - lineStart.
y()) * (lineEnd.
y() - lineStart.
y()))
299 ) / lineLengthSquared;
300 if (u < 0.0f || u > 1.0f) {
304 lineStart.
x() + u * (lineEnd.
x() - lineStart.
x()),
305 lineStart.
y() + u * (lineEnd.
y() - lineStart.
y()));
317 outIntersection.
set(
Line(lineStart, lineEnd).getPositionAtDistance2D(length));
328 const SUMOReal dx = lineBeg.
x() - lineEnd.
x();
329 const SUMOReal dy = lineBeg.
y() - lineEnd.
y();
330 const SUMOReal length = sqrt(dx * dx + dy * dy);
332 p.
add(dy * amount / length, -dx * amount / length);
364 std::pair<SUMOReal, SUMOReal>
372 std::pair<SUMOReal, SUMOReal>
379 return std::pair<SUMOReal, SUMOReal>
380 ((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.
static SUMOReal closestDistancePointLine2D(const Position &point, const Position &lineStart, const Position &lineEnd, Position &outIntersection)
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 const SUMOReal INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
static const Position INVALID