GeomHelper.cpp

/****************************************************************************/
// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
// Copyright (C) 2001-2025 German Aerospace Center (DLR) and others.
// This program and the accompanying materials are made available under the
// terms of the Eclipse Public License 2.0 which is available at
// https://www.eclipse.org/legal/epl-2.0/
// This Source Code may also be made available under the following Secondary
// Licenses when the conditions for such availability set forth in the Eclipse
// Public License 2.0 are satisfied: GNU General Public License, version 2
// or later which is available at
// https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
// SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
/****************************************************************************/
/// @file    GeomHelper.cpp
/// @author  Daniel Krajzewicz
/// @author  Friedemann Wesner
/// @author  Jakob Erdmann
/// @author  Michael Behrisch
/// @author  Mirko Barthauer
/// @date    Sept 2002
///
// Some static methods performing geometrical operations
/****************************************************************************/
#include <config.h>

#include <cmath>
#include <limits>
#include <algorithm>
#include <iostream>
#include <utils/common/StdDefs.h>
#include <utils/common/ToString.h>
#include <utils/common/MsgHandler.h>
#include "Boundary.h"
#include "GeomHelper.h"

// ===========================================================================
// static members
// ===========================================================================
const double GeomHelper::INVALID_OFFSET = -1;


// ===========================================================================
// method definitions
// ===========================================================================

void
GeomHelper::findLineCircleIntersections(const Position& c, double radius, const Position& p1, const Position& p2,
                                        std::vector<double>& into) {
    const double dx = p2.x() - p1.x();
    const double dy = p2.y() - p1.y();

    const double A = dx * dx + dy * dy;
    const double B = 2 * (dx * (p1.x() - c.x()) + dy * (p1.y() - c.y()));
    const double C = (p1.x() - c.x()) * (p1.x() - c.x()) + (p1.y() - c.y()) * (p1.y() - c.y()) - radius * radius;

    const double det = B * B - 4 * A * C;
    if ((A <= 0.0000001) || (det < 0)) {
        // No real solutions.
        return;
    }
    if (det == 0) {
        // One solution.
        const double t = -B / (2 * A);
        if (t >= 0. && t <= 1.) {
            into.push_back(t);
        }
    } else {
        // Two solutions.
        const double t = (double)((-B + sqrt(det)) / (2 * A));
        Position intersection(p1.x() + t * dx, p1.y() + t * dy);
        if (t >= 0. && t <= 1.) {
            into.push_back(t);
        }
        const double t2 = (double)((-B - sqrt(det)) / (2 * A));
        if (t2 >= 0. && t2 <= 1.) {
            into.push_back(t2);
        }
    }
}


double
GeomHelper::angle2D(const Position& p1, const Position& p2) {
    return angleDiff(atan2(p1.y(), p1.x()), atan2(p2.y(), p2.x()));
}


double
GeomHelper::nearest_offset_on_line_to_point2D(const Position& lineStart,
        const Position& lineEnd,
        const Position& p, bool perpendicular) {
    const double lineLength2D = lineStart.distanceTo2D(lineEnd);
    if (lineLength2D == 0.) {
        return 0.;
    }
    // scalar product equals length of orthogonal projection times length of vector being projected onto
    // dividing the scalar product by the distance gives the relative position
    const double u = (((p.x() - lineStart.x()) * (lineEnd.x() - lineStart.x())) +
                      ((p.y() - lineStart.y()) * (lineEnd.y() - lineStart.y()))
                     ) / lineLength2D;
    if (u < 0. || u > lineLength2D) {  // closest point does not fall within the line segment
        if (perpendicular) {
            return INVALID_OFFSET;
        }
        if (u < 0.) {
            return 0.;
        }
        return lineLength2D;
    }
    return u;
}


double
GeomHelper::nearest_offset_on_line_to_point25D(const Position& lineStart,
        const Position& lineEnd,
        const Position& p, bool perpendicular) {
    double result = nearest_offset_on_line_to_point2D(lineStart, lineEnd, p, perpendicular);
    if (result != INVALID_OFFSET) {
        const double lineLength2D = lineStart.distanceTo2D(lineEnd);
        const double lineLength = lineStart.distanceTo(lineEnd);
        result *= (lineLength / lineLength2D);
    }
    return result;
}

Position
GeomHelper::crossPoint(const Boundary& b, const PositionVector& v) {
    if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmin(), b.ymax()))) {
        return v.intersectionPosition2D(
                   Position(b.xmin(), b.ymin()),
                   Position(b.xmin(), b.ymax()));
    }
    if (v.intersects(Position(b.xmax(), b.ymin()), Position(b.xmax(), b.ymax()))) {
        return v.intersectionPosition2D(
                   Position(b.xmax(), b.ymin()),
                   Position(b.xmax(), b.ymax()));
    }
    if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmax(), b.ymin()))) {
        return v.intersectionPosition2D(
                   Position(b.xmin(), b.ymin()),
                   Position(b.xmax(), b.ymin()));
    }
    if (v.intersects(Position(b.xmin(), b.ymax()), Position(b.xmax(), b.ymax()))) {
        return v.intersectionPosition2D(
                   Position(b.xmin(), b.ymax()),
                   Position(b.xmax(), b.ymax()));
    }
    throw 1;
}

double
GeomHelper::getCCWAngleDiff(double angle1, double angle2) {
    double v = angle2 - angle1;
    if (v < 0) {
        v = 360 + v;
    }
    return v;
}


double
GeomHelper::getCWAngleDiff(double angle1, double angle2) {
    double v = angle1 - angle2;
    if (v < 0) {
        v = 360 + v;
    }
    return v;
}


double
GeomHelper::getMinAngleDiff(double angle1, double angle2) {
    return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2));
}


double
GeomHelper::angleDiff(const double angle1, const double angle2) {
    double dtheta = angle2 - angle1;
    while (dtheta > (double) M_PI) {
        dtheta -= (double)(2.0 * M_PI);
    }
    while (dtheta < (double) - M_PI) {
        dtheta += (double)(2.0 * M_PI);
    }
    return dtheta;
}


double
GeomHelper::naviDegree(const double angle) {
    double degree = RAD2DEG(M_PI / 2. - angle);
    if (std::isinf(degree)) {
        //assert(false);
        return 0;
    }
    while (degree >= 360.) {
        degree -= 360.;
    }
    while (degree < 0.) {
        degree += 360.;
    }
    return degree;
}


double
GeomHelper::fromNaviDegree(const double angle) {
    return M_PI / 2. - DEG2RAD(angle);
}


double
GeomHelper::legacyDegree(const double angle, const bool positive) {
    double degree = -RAD2DEG(M_PI / 2. + angle);
    if (positive) {
        while (degree >= 360.) {
            degree -= 360.;
        }
        while (degree < 0.) {
            degree += 360.;
        }
    } else {
        while (degree >= 180.) {
            degree -= 360.;
        }
        while (degree < -180.) {
            degree += 360.;
        }
    }
    return degree;
}

PositionVector
GeomHelper::makeCircle(const double radius, const Position& center, unsigned int nPoints) {
    if (nPoints < 3) {
        WRITE_ERROR(TL("GeomHelper::makeCircle() requires nPoints>=3"));
    }
    PositionVector circle;
    circle.push_back({radius, 0});
    for (unsigned int i = 1; i < nPoints; ++i) {
        const double a = 2.0 * M_PI * (double)i / (double) nPoints;
        circle.push_back({radius * cos(a), radius * sin(a)});
    }
    circle.push_back({radius, 0});
    circle.add(center);
    return circle;
}


PositionVector
GeomHelper::makeRing(const double radius1, const double radius2, const Position& center, unsigned int nPoints) {
    if (nPoints < 3) {
        WRITE_ERROR("GeomHelper::makeRing() requires nPoints>=3");
    }
    if (radius1 >= radius2) {
        WRITE_ERROR("GeomHelper::makeRing() requires radius2>radius1");
    }
    PositionVector ring;
    ring.push_back({radius1, 0});
    ring.push_back({radius2, 0});
    for (unsigned int i = 1; i < nPoints; ++i) {
        const double a = 2.0 * M_PI * (double)i / (double) nPoints;
        ring.push_back({radius2 * cos(a), radius2 * sin(a)});
    }
    ring.push_back({radius2, 0});
    ring.push_back({radius1, 0});
    for (unsigned int i = 1; i < nPoints; ++i) {
        const double a = -2.0 * M_PI * (double)i / (double) nPoints;
        ring.push_back({radius1 * cos(a), radius1 * sin(a)});
    }
    ring.push_back({radius1, 0});
    ring.add(center);
    return ring;
}



const Position
GeomHelper::calculateLotSpacePosition(const PositionVector& shape, const int index, const double spaceDim, const double angle,
                                      const double width, const double length) {
    //declare pos
    Position pos;
    // declare shape offsets
    const Position startOffset = shape.positionAtOffset(spaceDim * (index));
    const Position endOffset = shape.positionAtOffset(spaceDim * (index + 1));
    // continue depending of nagle
    if (angle == 0) {
        // parking parallel to the road
        pos = endOffset;
    } else {
        // angled parking
        double normAngle = angle;
        while (normAngle < 0) {
            normAngle += 360.;
        }
        normAngle = fmod(normAngle, 360);
        const double radianAngle = normAngle / 180 * M_PI;
        double spaceExtension = width * sin(radianAngle) + length * cos(radianAngle);
        const double hlp_angle = fabs(((double)atan2((endOffset.y() - startOffset.y()), (endOffset.x() - startOffset.x()))));
        Position offset;
        double xOffset = 0.5 * width * sin(radianAngle) - 0.5 * (spaceExtension - spaceDim);
        pos.setx(startOffset.x() + xOffset + length * cos(radianAngle));
        if (normAngle <= 90) {
            pos.sety((startOffset.y() + 0.5 * width * (1 - cos(radianAngle)) - length * sin(radianAngle)));
        } else if (normAngle <= 180) {
            pos.sety(startOffset.y() + 0.5 * width * (1 + cos(radianAngle)) - length * sin(radianAngle));
        } else if (angle <= 270) {
            pos.sety(startOffset.y() + 0.5 * width * (1 - cos(radianAngle - M_PI)));
        } else {
            pos.sety(startOffset.y() + 0.5 * width * (1 + cos(radianAngle - M_PI)));
        }
        pos.setz((startOffset.z() + endOffset.z()) / 2);
        pos = pos.rotateAround2D(hlp_angle, startOffset);
    }
    return pos;
}


double
GeomHelper::calculateLotSpaceAngle(const PositionVector& shape, const int index, const double spaceDim, const double angle) {
    // declare shape offsets
    const Position startOffset = shape.positionAtOffset(spaceDim * (index));
    const Position endOffset = shape.positionAtOffset(spaceDim * (index + 1));
    // return angle
    return ((double)atan2((endOffset.x() - startOffset.x()), (startOffset.y() - endOffset.y())) * (double)180.0 / (double)M_PI) - angle;
}


double
GeomHelper::calculateLotSpaceSlope(const PositionVector& shape, const int index, const double spaceDim) {
    return shape.slopeDegreeAtOffset(spaceDim * (index + 1));
}

/****************************************************************************/