48 std::vector<double>& into) {
49 const double dx = p2.
x() - p1.
x();
50 const double dy = p2.
y() - p1.
y();
52 const double A = dx * dx + dy * dy;
53 const double B = 2 * (dx * (p1.
x() - c.
x()) + dy * (p1.
y() - c.
y()));
54 const double C = (p1.
x() - c.
x()) * (p1.
x() - c.
x()) + (p1.
y() - c.
y()) * (p1.
y() - c.
y()) - radius * radius;
56 const double det = B * B - 4 * A * C;
57 if ((A <= 0.0000001) || (det < 0)) {
63 const double t = -B / (2 * A);
64 if (t >= 0. && t <= 1.) {
69 const double t = (double)((-B + sqrt(det)) / (2 * A));
70 Position intersection(p1.
x() + t * dx, p1.
y() + t * dy);
71 if (t >= 0. && t <= 1.) {
74 const double t2 = (double)((-B - sqrt(det)) / (2 * A));
75 if (t2 >= 0. && t2 <= 1.) {
84 return angleDiff(atan2(p1.
y(), p1.
x()), atan2(p2.
y(), p2.
x()));
91 const Position& p,
bool perpendicular) {
92 const double lineLength2D = lineStart.
distanceTo2D(lineEnd);
93 if (lineLength2D == 0.0f) {
98 const double u = (((p.
x() - lineStart.
x()) * (lineEnd.
x() - lineStart.
x())) +
99 ((p.
y() - lineStart.
y()) * (lineEnd.
y() - lineStart.
y()))
100 ) / (lineLength2D * lineLength2D);
101 if (u < 0.0f || u > 1.0f) {
110 return u * lineLength2D;
142 double v = angle2 - angle1;
152 double v = angle1 - angle2;
168 double dtheta = angle2 - angle1;
169 while (dtheta > (
double)
M_PI) {
170 dtheta -= (double)(2.0 * M_PI);
172 while (dtheta < (
double) -
M_PI) {
173 dtheta += (double)(2.0 * M_PI);
182 if (std::isinf(degree)) {
186 while (degree >= 360.) {
189 while (degree < 0.) {
206 while (degree >= 360.) {
209 while (degree < 0.) {
213 while (degree >= 180.) {
216 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.