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