49 std::vector<double>& into) {
50 const double dx = p2.
x() - p1.
x();
51 const double dy = p2.
y() - p1.
y();
53 const double A = dx * dx + dy * dy;
54 const double B = 2 * (dx * (p1.
x() - c.
x()) + dy * (p1.
y() - c.
y()));
55 const double C = (p1.
x() - c.
x()) * (p1.
x() - c.
x()) + (p1.
y() - c.
y()) * (p1.
y() - c.
y()) - radius * radius;
57 const double det = B * B - 4 * A * C;
58 if ((A <= 0.0000001) || (det < 0)) {
64 const double t = -B / (2 * A);
65 if (t >= 0. && t <= 1.) {
70 const double t = (double)((-B + sqrt(det)) / (2 * A));
71 Position intersection(p1.
x() + t * dx, p1.
y() + t * dy);
72 if (t >= 0. && t <= 1.) {
75 const double t2 = (double)((-B - sqrt(det)) / (2 * A));
76 if (t2 >= 0. && t2 <= 1.) {
85 return angleDiff(atan2(p1.
y(), p1.
x()), atan2(p2.
y(), p2.
x()));
92 const Position& p,
bool perpendicular) {
93 const double lineLength2D = lineStart.
distanceTo2D(lineEnd);
94 if (lineLength2D == 0.0f) {
99 const double u = (((p.
x() - lineStart.
x()) * (lineEnd.
x() - lineStart.
x())) +
100 ((p.
y() - lineStart.
y()) * (lineEnd.
y() - lineStart.
y()))
101 ) / (lineLength2D * lineLength2D);
102 if (u < 0.0f || u > 1.0f) {
111 return u * lineLength2D;
119 const Position& p,
bool perpendicular) {
122 const double lineLength2D = lineStart.
distanceTo2D(lineEnd);
123 const double lineLength = lineStart.
distanceTo(lineEnd);
124 result *= (lineLength / lineLength2D);
156 double v = angle2 - angle1;
166 double v = angle1 - angle2;
182 double dtheta = angle2 - angle1;
183 while (dtheta > (
double)
M_PI) {
184 dtheta -= (double)(2.0 * M_PI);
186 while (dtheta < (
double) -
M_PI) {
187 dtheta += (double)(2.0 * M_PI);
196 if (std::isinf(degree)) {
200 while (degree >= 360.) {
203 while (degree < 0.) {
220 while (degree >= 360.) {
223 while (degree < 0.) {
227 while (degree >= 180.) {
230 while (degree < -180.) {
240 WRITE_ERROR(
"GeomHelper::makeCircle() requires nPoints>=3");
243 circle.push_back({radius, 0});
244 for (
unsigned int i = 1; i < nPoints; ++i) {
245 const double a = 2.0 *
M_PI * (double)i / (
double) nPoints;
246 circle.push_back({radius * cos(a), radius * sin(a)});
248 circle.push_back({radius, 0});
257 WRITE_ERROR(
"GeomHelper::makeRing() requires nPoints>=3");
259 if (radius1 >= radius2) {
260 WRITE_ERROR(
"GeomHelper::makeRing() requires radius2>radius1");
263 ring.push_back({radius1, 0});
264 ring.push_back({radius2, 0});
265 for (
unsigned int i = 1; i < nPoints; ++i) {
266 const double a = 2.0 *
M_PI * (double)i / (
double) nPoints;
267 ring.push_back({radius2 * cos(a), radius2 * sin(a)});
269 ring.push_back({radius2, 0});
270 ring.push_back({radius1, 0});
271 for (
unsigned int i = 1; i < nPoints; ++i) {
272 const double a = -2.0 *
M_PI * (double)i / (
double) nPoints;
273 ring.push_back({radius1 * cos(a), radius1 * sin(a)});
275 ring.push_back({radius1, 0});
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.
static PositionVector makeRing(const double radius1, const double radius2, const Position ¢er, unsigned int nPoints)
double x() const
Returns the x-position.
static Position crossPoint(const Boundary &b, const PositionVector &v)
static double nearest_offset_on_line_to_point25D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
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)
static PositionVector makeCircle(const double radius, const Position ¢er, unsigned int nPoints)
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.
double distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
void add(double xoff, double yoff, double zoff)
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.