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.) {
97 const double u = (((p.
x() - lineStart.
x()) * (lineEnd.
x() - lineStart.
x())) +
98 ((p.
y() - lineStart.
y()) * (lineEnd.
y() - lineStart.
y()))
100 if (u < 0. || u > lineLength2D) {
116 const Position& p,
bool perpendicular) {
119 const double lineLength2D = lineStart.
distanceTo2D(lineEnd);
120 const double lineLength = lineStart.
distanceTo(lineEnd);
121 result *= (lineLength / lineLength2D);
153 double v = angle2 - angle1;
163 double v = angle1 - angle2;
179 double dtheta = angle2 - angle1;
180 while (dtheta > (
double)
M_PI) {
181 dtheta -= (double)(2.0 *
M_PI);
183 while (dtheta < (
double) -
M_PI) {
184 dtheta += (double)(2.0 *
M_PI);
193 if (std::isinf(degree)) {
197 while (degree >= 360.) {
200 while (degree < 0.) {
217 while (degree >= 360.) {
220 while (degree < 0.) {
224 while (degree >= 180.) {
227 while (degree < -180.) {
237 WRITE_ERROR(
TL(
"GeomHelper::makeCircle() requires nPoints>=3"));
240 circle.push_back({radius, 0});
241 for (
unsigned int i = 1; i < nPoints; ++i) {
242 const double a = 2.0 *
M_PI * (double)i / (
double) nPoints;
243 circle.push_back({radius * cos(a), radius * sin(a)});
245 circle.push_back({radius, 0});
254 WRITE_ERROR(
"GeomHelper::makeRing() requires nPoints>=3");
256 if (radius1 >= radius2) {
257 WRITE_ERROR(
"GeomHelper::makeRing() requires radius2>radius1");
260 ring.push_back({radius1, 0});
261 ring.push_back({radius2, 0});
262 for (
unsigned int i = 1; i < nPoints; ++i) {
263 const double a = 2.0 *
M_PI * (double)i / (
double) nPoints;
264 ring.push_back({radius2 * cos(a), radius2 * sin(a)});
266 ring.push_back({radius2, 0});
267 ring.push_back({radius1, 0});
268 for (
unsigned int i = 1; i < nPoints; ++i) {
269 const double a = -2.0 *
M_PI * (double)i / (
double) nPoints;
270 ring.push_back({radius1 * cos(a), radius1 * sin(a)});
272 ring.push_back({radius1, 0});
281 const double width,
const double length) {
293 const double hlp_angle = fabs(((
double)atan2((endOffset.
x() - startOffset.
x()), (startOffset.
y() - endOffset.
y())) * (
double)180.0 / (
double)
M_PI) - 180);
294 if (angle >= 0 && angle <= 90) {
295 pos.
setx((startOffset.
x() + endOffset.
x()) / 2 - (width / 2) * (1 - cos(angle / 180 *
M_PI)) * cos(hlp_angle / 180 *
M_PI));
296 pos.
sety((startOffset.
y() + endOffset.
y()) / 2 + (width / 2) * (1 - cos(angle / 180 *
M_PI)) * sin(hlp_angle / 180 *
M_PI));
297 pos.
setz((startOffset.
z() + endOffset.
z()) / 2);
298 }
else if (angle > 90 && angle <= 180) {
299 pos.
setx((startOffset.
x() + endOffset.
x()) / 2 - (width / 2) * (1 + cos(angle / 180 *
M_PI)) * cos(hlp_angle / 180 *
M_PI));
300 pos.
sety((startOffset.
y() + endOffset.
y()) / 2 + (width / 2) * (1 + cos(angle / 180 *
M_PI)) * sin(hlp_angle / 180 *
M_PI));
301 pos.
setz((startOffset.
z() + endOffset.
z()) / 2);
302 }
else if (angle > 180 && angle <= 270) {
303 pos.
setx((startOffset.
x() + endOffset.
x()) / 2 - (length)*sin((angle - hlp_angle) / 180 *
M_PI) - (width / 2) * (1 + cos(angle / 180 *
M_PI)) * cos(hlp_angle / 180 *
M_PI));
304 pos.
sety((startOffset.
y() + endOffset.
y()) / 2 + (length)*cos((angle - hlp_angle) / 180 *
M_PI) + (width / 2) * (1 + cos(angle / 180 *
M_PI)) * sin(hlp_angle / 180 *
M_PI));
305 pos.
setz((startOffset.
z() + endOffset.
z()) / 2);
306 }
else if (angle > 270 && angle < 360) {
307 pos.
setx((startOffset.
x() + endOffset.
x()) / 2 - (length)*sin((angle - hlp_angle) / 180 *
M_PI) - (width / 2) * (1 - cos(angle / 180 *
M_PI)) * cos(hlp_angle / 180 *
M_PI));
308 pos.
sety((startOffset.
y() + endOffset.
y()) / 2 + (length)*cos((angle - hlp_angle) / 180 *
M_PI) + (width / 2) * (1 - cos(angle / 180 *
M_PI)) * sin(hlp_angle / 180 *
M_PI));
309 pos.
setz((startOffset.
z() + endOffset.
z()) / 2);
311 pos = (startOffset + endOffset) * 0.5;
324 return ((
double)atan2((endOffset.
x() - startOffset.
x()), (startOffset.
y() - endOffset.
y())) * (
double)180.0 / (
double)
M_PI) + angle;
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 const Position calculateLotSpacePosition(const PositionVector &shape, const int index, const double spaceDim, const double angle, const double width, const double length)
calculate lotSpace position
static double calculateLotSpaceAngle(const PositionVector &shape, const int index, const double spaceDim, const double angle)
calculate lotSpace angle
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 calculateLotSpaceSlope(const PositionVector &shape, const int index, const double spaceDim)
calculate lotSpace slope
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.
void setx(double x)
set position x
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 dimensions
double x() const
Returns the x-position.
void setz(double z)
set position z
double z() const
Returns the z-position.
void sety(double y)
set position y
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.
Position positionAtOffset(double pos, double lateralOffset=0) const
Returns the position at the given length.
void add(double xoff, double yoff, double zoff)
double slopeDegreeAtOffset(double pos) const
Returns the slope at the given length.
bool intersects(const Position &p1, const Position &p2) const
Returns the information whether this list of points interesects the given line.