54 std::vector<double>& into) {
55 const double dx = p2.
x() - p1.
x();
56 const double dy = p2.
y() - p1.
y();
58 const double A = dx * dx + dy * dy;
59 const double B = 2 * (dx * (p1.
x() - c.
x()) + dy * (p1.
y() - c.
y()));
60 const double C = (p1.
x() - c.
x()) * (p1.
x() - c.
x()) + (p1.
y() - c.
y()) * (p1.
y() - c.
y()) - radius * radius;
62 const double det = B * B - 4 * A * C;
63 if ((A <= 0.0000001) || (det < 0)) {
69 const double t = -B / (2 * A);
70 if (t >= 0. && t <= 1.) {
75 const double t = (double)((-B + sqrt(det)) / (2 * A));
76 Position intersection(p1.
x() + t * dx, p1.
y() + t * dy);
77 if (t >= 0. && t <= 1.) {
80 const double t2 = (double)((-B - sqrt(det)) / (2 * A));
81 if (t2 >= 0. && t2 <= 1.) {
90 return angleDiff(atan2(p1.
y(), p1.
x()), atan2(p2.
y(), p2.
x()));
97 const Position& p,
bool perpendicular) {
98 const double lineLength2D = lineStart.
distanceTo2D(lineEnd);
99 if (lineLength2D == 0.0f) {
104 const double u = (((p.
x() - lineStart.
x()) * (lineEnd.
x() - lineStart.
x())) +
105 ((p.
y() - lineStart.
y()) * (lineEnd.
y() - lineStart.
y()))
106 ) / (lineLength2D * lineLength2D);
107 if (u < 0.0f || u > 1.0f) {
116 return u * lineLength2D;
148 double v = angle2 - angle1;
158 double v = angle1 - angle2;
174 double dtheta = angle2 - angle1;
175 while (dtheta > (
double)
M_PI) {
176 dtheta -= (double)(2.0 * M_PI);
178 while (dtheta < (
double) -
M_PI) {
179 dtheta += (double)(2.0 * M_PI);
188 while (degree >= 360.) {
191 while (degree < 0.) {
208 while (degree >= 360.) {
211 while (degree < 0.) {
215 while (degree >= 180.) {
218 while (degree < -180.) {
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
double ymin() const
Returns minimum y-coordinate.
double xmax() const
Returns maximum x-coordinate.
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Position intersectionPosition2D(const Position &p1, const Position &p2, const double withinDist=0.) const
Returns the position of the intersection.
double y() const
Returns the y-position.
static double getCCWAngleDiff(double angle1, double angle2)
Returns the distance of second angle from first angle counter-clockwise.
double x() const
Returns the x-position.
static Position crossPoint(const Boundary &b, const PositionVector &v)
A class that stores a 2D geometrical boundary.
static double legacyDegree(const double angle, const bool positive=false)
static double naviDegree(const double angle)
static double nearest_offset_on_line_to_point2D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
A point in 2D or 3D with translation and scaling methods.
static double getCWAngleDiff(double angle1, double angle2)
Returns the distance of second angle from first angle clockwise.
static double angle2D(const Position &p1, const Position &p2)
Returns the angle between two vectors on a plane The angle is from vector 1 to vector 2...
static double fromNaviDegree(const double angle)
double xmin() const
Returns minimum x-coordinate.
static const double INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
static void findLineCircleIntersections(const Position &c, double radius, const Position &p1, const Position &p2, std::vector< double > &into)
Returns the positions the given circle is crossed by the given line.
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
double ymax() const
Returns maximum y-coordinate.
bool intersects(const Position &p1, const Position &p2) const
Returns the information whether this list of points interesects the given line.