47 std::vector<double>& into) {
48 const double dx = p2.
x() - p1.
x();
49 const double dy = p2.
y() - p1.
y();
51 const double A = dx * dx + dy * dy;
52 const double B = 2 * (dx * (p1.
x() - c.
x()) + dy * (p1.
y() - c.
y()));
53 const double C = (p1.
x() - c.
x()) * (p1.
x() - c.
x()) + (p1.
y() - c.
y()) * (p1.
y() - c.
y()) - radius * radius;
55 const double det = B * B - 4 * A * C;
56 if ((A <= 0.0000001) || (det < 0)) {
62 const double t = -B / (2 * A);
63 if (t >= 0. && t <= 1.) {
68 const double t = (double)((-B + sqrt(det)) / (2 * A));
69 Position intersection(p1.
x() + t * dx, p1.
y() + t * dy);
70 if (t >= 0. && t <= 1.) {
73 const double t2 = (double)((-B - sqrt(det)) / (2 * A));
74 if (t2 >= 0. && t2 <= 1.) {
83 return angleDiff(atan2(p1.
y(), p1.
x()), atan2(p2.
y(), p2.
x()));
90 const Position& p,
bool perpendicular) {
91 const double lineLength2D = lineStart.
distanceTo2D(lineEnd);
92 if (lineLength2D == 0.0f) {
97 const double u = (((p.
x() - lineStart.
x()) * (lineEnd.
x() - lineStart.
x())) +
98 ((p.
y() - lineStart.
y()) * (lineEnd.
y() - lineStart.
y()))
99 ) / (lineLength2D * lineLength2D);
100 if (u < 0.0f || u > 1.0f) {
109 return u * lineLength2D;
117 const Position& p,
bool perpendicular) {
120 const double lineLength2D = lineStart.
distanceTo2D(lineEnd);
121 const double lineLength = lineStart.
distanceTo(lineEnd);
122 result *= (lineLength / lineLength2D);
154 double v = angle2 - angle1;
164 double v = angle1 - angle2;
180 double dtheta = angle2 - angle1;
181 while (dtheta > (
double)
M_PI) {
182 dtheta -= (double)(2.0 *
M_PI);
184 while (dtheta < (
double) -
M_PI) {
185 dtheta += (double)(2.0 *
M_PI);
194 if (std::isinf(degree)) {
198 while (degree >= 360.) {
201 while (degree < 0.) {
218 while (degree >= 360.) {
221 while (degree < 0.) {
225 while (degree >= 180.) {
228 while (degree < -180.) {
238 WRITE_ERROR(
"GeomHelper::makeCircle() requires nPoints>=3");
241 circle.push_back({radius, 0});
242 for (
unsigned int i = 1; i < nPoints; ++i) {
243 const double a = 2.0 *
M_PI * (double)i / (
double) nPoints;
244 circle.push_back({radius * cos(a), radius * sin(a)});
246 circle.push_back({radius, 0});
255 WRITE_ERROR(
"GeomHelper::makeRing() requires nPoints>=3");
257 if (radius1 >= radius2) {
258 WRITE_ERROR(
"GeomHelper::makeRing() requires radius2>radius1");
261 ring.push_back({radius1, 0});
262 ring.push_back({radius2, 0});
263 for (
unsigned int i = 1; i < nPoints; ++i) {
264 const double a = 2.0 *
M_PI * (double)i / (
double) nPoints;
265 ring.push_back({radius2 * cos(a), radius2 * sin(a)});
267 ring.push_back({radius2, 0});
268 ring.push_back({radius1, 0});
269 for (
unsigned int i = 1; i < nPoints; ++i) {
270 const double a = -2.0 *
M_PI * (double)i / (
double) nPoints;
271 ring.push_back({radius1 * cos(a), radius1 * sin(a)});
273 ring.push_back({radius1, 0});
A class that stores a 2D geometrical boundary.
double ymin() const
Returns minimum y-coordinate.
double xmin() const
Returns minimum x-coordinate.
double ymax() const
Returns maximum y-coordinate.
double xmax() const
Returns maximum x-coordinate.
static double getCCWAngleDiff(double angle1, double angle2)
Returns the distance of second angle from first angle counter-clockwise.
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 nearest_offset_on_line_to_point25D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
static Position crossPoint(const Boundary &b, const PositionVector &v)
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 PositionVector makeRing(const double radius1, const double radius2, const Position ¢er, unsigned int nPoints)
static const double INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
static double getCWAngleDiff(double angle1, double angle2)
Returns the distance of second angle from first angle clockwise.
static double nearest_offset_on_line_to_point2D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
static PositionVector makeCircle(const double radius, const Position ¢er, unsigned int nPoints)
static double naviDegree(const double angle)
static double fromNaviDegree(const double angle)
static double legacyDegree(const double angle, const bool positive=false)
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
A point in 2D or 3D with translation and scaling methods.
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
double distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
double x() const
Returns the x-position.
double y() const
Returns the y-position.
Position intersectionPosition2D(const Position &p1, const Position &p2, const double withinDist=0.) const
Returns the position of the intersection.
void add(double xoff, double yoff, double zoff)
bool intersects(const Position &p1, const Position &p2) const
Returns the information whether this list of points interesects the given line.