geometry/LineString.h
Go to the documentation of this file.
1 #pragma once
2 #include <boost/geometry/algorithms/intersects.hpp>
3 #include <boost/geometry/algorithms/length.hpp>
4 #include <boost/geometry/geometries/register/linestring.hpp>
5 #include <boost/geometry/geometries/register/segment.hpp>
6 
13 
14 /***********************************************************************
15  * BOOST GEOMETRY REGISTRATIONS *
16  ***********************************************************************/
17 
18 BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::BasicLineString3d)
19 BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::BasicLineString2d)
20 BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::LineString3d)
21 BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::LineString2d)
22 BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::ConstLineString3d)
23 BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::ConstLineString2d)
24 BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::ConstHybridLineString3d)
25 BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::ConstHybridLineString2d)
26 BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::CompoundLineString2d)
27 BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::CompoundLineString3d)
28 BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::CompoundHybridLineString2d)
29 BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::CompoundHybridLineString3d)
30 BOOST_GEOMETRY_REGISTER_SEGMENT_TEMPLATIZED(lanelet::Segment, first, second)
31 
32 namespace lanelet {
33 namespace geometry {
34 
35 using boost::geometry::intersects;
36 using boost::geometry::length;
37 using boost::geometry::overlaps;
38 using boost::geometry::touches;
39 
40 template <typename LineStringIterator>
41 double rangedLength(LineStringIterator start, LineStringIterator end);
42 
54 template <typename LineStringT>
55 std::vector<double> lengthRatios(const LineStringT& lineString);
56 
66 template <typename LineStringT>
67 std::vector<double> accumulatedLengthRatios(const LineStringT& lineString);
68 
82 template <typename LineString3dT>
83 double signedDistance(const LineString3dT& lineString, const BasicPoint3d& p);
84 
98 template <typename LineString2dT>
99 double signedDistance(const LineString2dT& lineString, const BasicPoint2d& p);
100 
110 template <typename Point2dT>
111 double curvature2d(const Point2dT& p1, const Point2dT& p2, const Point2dT& p3);
112 
122 template <typename Point2dT>
123 double signedCurvature2d(const Point2dT& p1, const Point2dT& p2, const Point2dT& p3);
124 
133 template <typename LineString2dT>
134 ArcCoordinates toArcCoordinates(const LineString2dT& lineString, const BasicPoint2d& point);
135 
148 template <typename LineStringT>
149 traits::BasicPointT<traits::PointType<LineStringT>> interpolatedPointAtDistance(LineStringT lineString, double dist);
150 
162 template <typename LineStringT>
163 traits::PointType<LineStringT> nearestPointAtDistance(LineStringT lineString, double dist);
164 
166 template <typename LineString3dT>
167 IfLS<LineString3dT, BoundingBox3d> boundingBox3d(const LineString3dT& lineString);
168 
170 template <typename LineString2dT>
171 IfLS<LineString2dT, BoundingBox2d> boundingBox2d(const LineString2dT& lineString);
172 
181 template <typename LineString3dT, typename = std::enable_if_t<traits::is3D<LineString3dT>()>>
182 BasicPoint3d project(const LineString3dT& lineString, const BasicPoint3d& pointToProject);
183 
185 template <typename LineString2dT, typename = std::enable_if_t<traits::is2D<LineString2dT>()>>
186 BasicPoint2d project(const LineString2dT& lineString, const BasicPoint2d& pointToProject);
187 
188 BasicPoint2d project(const BasicSegment2d& segment, const BasicPoint2d& pointToProject);
189 BasicPoint3d project(const BasicSegment3d& segment, const BasicPoint3d& pointToProject);
190 
197 template <typename LineString2dT>
198 IfLS<LineString2dT, std::pair<BasicPoint2d, BasicPoint2d>> projectedPoint2d(const LineString2dT& l1,
199  const LineString2dT& l2);
200 
207 template <typename LineString3dT>
208 IfLS<LineString3dT, std::pair<BasicPoint3d, BasicPoint3d>> projectedPoint3d(const LineString3dT& l1,
209  const LineString3dT& l2);
210 
218 template <typename LineString3dT>
219 IfLS<LineString3dT, bool> intersects3d(const LineString3dT& linestring, const LineString3dT& otherLinestring,
220  double heightTolerance = 3.);
221 
240 template <typename LineString1T, typename LineString2T>
241 std::pair<LineString1T, LineString2T> align(LineString1T left, LineString2T right);
242 
248 template <typename LineString2dT>
249 BasicPoint2d fromArcCoordinates(const LineString2dT& lineString, const ArcCoordinates& arcCoords);
250 
264 template <typename LineString2dT>
265 BasicLineString2d offset(const LineString2dT& lineString, double distance);
266 
278 template <typename LineString2dT>
279 BasicLineString2d offsetNoThrow(const LineString2dT& lineString, double distance);
280 
287 Segment<BasicPoint2d> closestSegment(const BasicLineString2d& lineString, const BasicPoint2d& pointToProject);
288 
295 Segment<BasicPoint3d> closestSegment(const BasicLineString3d& lineString, const BasicPoint3d& pointToProject);
296 Segment<ConstPoint2d> closestSegment(const ConstLineString2d& lineString, const BasicPoint2d& pointToProject);
297 Segment<ConstPoint3d> closestSegment(const ConstLineString3d& lineString, const BasicPoint3d& pointToProject);
298 Segment<BasicPoint2d> closestSegment(const ConstHybridLineString2d& lineString, const BasicPoint2d& pointToProject);
299 Segment<BasicPoint3d> closestSegment(const ConstHybridLineString3d& lineString, const BasicPoint3d& pointToProject);
300 Segment<ConstPoint2d> closestSegment(const CompoundLineString2d& lineString, const BasicPoint2d& pointToProject);
301 Segment<ConstPoint3d> closestSegment(const CompoundLineString3d& lineString, const BasicPoint3d& pointToProject);
302 Segment<BasicPoint2d> closestSegment(const CompoundHybridLineString2d& lineString, const BasicPoint2d& pointToProject);
303 Segment<BasicPoint3d> closestSegment(const CompoundHybridLineString3d& lineString, const BasicPoint3d& pointToProject);
304 } // namespace geometry
305 } // namespace lanelet
306 
307 #include "impl/LineString.h"
LineString.h
lanelet::geometry::boundingBox3d
IfAr< AreaT, BoundingBox3d > boundingBox3d(const AreaT &area)
calculates 3d bounding box
Definition: geometry/impl/Area.h:30
lanelet::CompoundHybridLineString2d
A hybrid compound linestring in 2d (returns BasicPoint2d)
Definition: CompoundLineString.h:306
lanelet::geometry::fromArcCoordinates
BasicPoint2d fromArcCoordinates(const LineString2dT &lineString, const ArcCoordinates &arcCoords)
create a point by moving laterally arcCoords.distance from the linestring point at arcCoords....
Definition: geometry/impl/LineString.h:832
lanelet::geometry::interpolatedPointAtDistance
traits::BasicPointT< traits::PointType< LineStringT > > interpolatedPointAtDistance(LineStringT lineString, double dist)
Definition: geometry/impl/LineString.h:641
lanelet::geometry::align
std::pair< LineString1T, LineString2T > align(LineString1T left, LineString2T right)
inverts the two linestrings such that they are parallel
Definition: geometry/impl/LineString.h:809
lanelet
Definition: Attribute.h:13
lanelet::BasicPoint3d
Eigen::Vector3d BasicPoint3d
a simple 3d-point
Definition: primitives/Point.h:19
GeometryHelper.h
lanelet::geometry::lengthRatios
std::vector< double > lengthRatios(const LineStringT &lineString)
Definition: geometry/impl/LineString.h:617
lanelet::geometry::project
BasicPoint3d project(const LineString3dT &lineString, const BasicPoint3d &pointToProject)
Projects the given point in 3d to the LineString.
Definition: geometry/impl/LineString.h:762
p2
BasicPoint p2
Definition: LineStringGeometry.cpp:167
p
BasicPoint p
Definition: LineStringGeometry.cpp:196
lanelet::BasicSegment3d
Segment< BasicPoint3d > BasicSegment3d
Definition: primitives/LineString.h:25
lanelet::geometry::intersects3d
IfLL< Lanelet1T, bool > intersects3d(const Lanelet1T &lanelet, const Lanelet2T &otherLanelet, double heightTolerance)
test whether two lanelets intersect in 2d.
Definition: geometry/impl/Lanelet.h:111
lanelet::Segment
std::pair< PointT, PointT > Segment
Definition: primitives/LineString.h:18
lanelet::geometry::offsetNoThrow
BasicLineString2d offsetNoThrow(const LineString2dT &lineString, const double distance)
create a linestring that is offset to the original one. the result.
Definition: geometry/impl/LineString.h:857
lanelet::ConstHybridLineString2d
A Linestring that returns BasicPoint2d instead of Point2d.
Definition: primitives/LineString.h:636
Point.h
lanelet::CompoundLineString3d
A Compound linestring in 3d (returns Point3d)
Definition: CompoundLineString.h:285
lanelet::ConstLineString2d
A normal 2d linestring with immutable data.
Definition: primitives/LineString.h:552
lanelet::LineString2d
A normal 2d linestring with mutable data.
Definition: primitives/LineString.h:569
lanelet::CompoundHybridLineString3d
A hybrid compound linestring in 3d (returns BasicPoint3d)
Definition: CompoundLineString.h:327
lanelet::BasicLineString2d
BasicPoints2d BasicLineString2d
Definition: primitives/LineString.h:14
lanelet::geometry::nearestPointAtDistance
traits::PointType< LineStringT > nearestPointAtDistance(LineStringT lineString, double dist)
returns the cosest point to a position on the linestring
Definition: geometry/impl/LineString.h:667
lanelet::geometry::projectedPoint2d
IfLS< LineString2dT, std::pair< BasicPoint2d, BasicPoint2d > > projectedPoint2d(const LineString2dT &l1, const LineString2dT &l2)
Computes the projected points on the two linestrings for the shortest distance.
Definition: geometry/impl/LineString.h:789
p1
BasicPoint p1
Definition: LineStringGeometry.cpp:166
lanelet::geometry::accumulatedLengthRatios
std::vector< double > accumulatedLengthRatios(const LineStringT &lineString)
Definition: geometry/impl/LineString.h:634
lanelet::geometry::signedDistance
double signedDistance(const LineString3dT &lineString, const BasicPoint3d &p)
Definition: geometry/impl/LineString.h:693
BoundingBox.h
lanelet::geometry::curvature2d
double curvature2d(const Point2dT &p1, const Point2dT &p2, const Point2dT &p3)
Definition: geometry/impl/LineString.h:705
lanelet::BasicPoint2d
Eigen::Matrix< double, 2, 1, Eigen::DontAlign > BasicPoint2d
a simple 2d-point
Definition: primitives/Point.h:20
lanelet::geometry::closestSegment
Segment< BasicPoint2d > closestSegment(const BasicLineString2d &lineString, const BasicPoint2d &pointToProject)
find the segment on a 2d line string that is closest to a given point, determined by boost::geometry:...
Definition: LineStringGeometry.cpp:442
lanelet::geometry::boundingBox2d
IfAr< AreaT, BoundingBox2d > boundingBox2d(const AreaT &area)
calculates an up-right 2d bounding box
Definition: geometry/impl/Area.h:25
lanelet::geometry::toArcCoordinates
ArcCoordinates toArcCoordinates(const LineString2dT &lineString, const BasicPoint2d &point)
Transform a point to the coordinates of the linestring.
Definition: geometry/impl/LineString.h:724
lanelet::CompoundLineString2d
A Compound linestring in 2d (returns Point2d)
Definition: CompoundLineString.h:263
lanelet::ConstLineString3d
A normal 3d linestring with immutable data.
Definition: primitives/LineString.h:521
LineString.h
distance
Optional< double > distance
Definition: LineStringGeometry.cpp:168
lanelet::ConstHybridLineString3d
A Linestring that returns BasicPoint3d instead of Point3d.
Definition: primitives/LineString.h:585
CompoundLineString.h
lanelet::geometry::signedCurvature2d
double signedCurvature2d(const Point2dT &p1, const Point2dT &p2, const Point2dT &p3)
Definition: geometry/impl/LineString.h:710
Forward.h
lanelet::LineString3d
A normal 3d linestring with mutable data.
Definition: primitives/LineString.h:538
lanelet::BasicSegment2d
Segment< BasicPoint2d > BasicSegment2d
Definition: primitives/LineString.h:24
lanelet::geometry::rangedLength
double rangedLength(LineStringIterator start, LineStringIterator end)
Definition: geometry/impl/LineString.h:610
lanelet::geometry::projectedPoint3d
IfLS< LineString3dT, std::pair< BasicPoint3d, BasicPoint3d > > projectedPoint3d(const LineString3dT &l1, const LineString3dT &l2)
Computes the projected points on the two linestrings for the shortest distance.
Definition: geometry/impl/LineString.h:796
lanelet::geometry::offset
BasicLineString2d offset(const LineString2dT &lineString, const double distance)
create a linestring that is offset to the original one. Guarantees no self-intersections and no inver...
Definition: geometry/impl/LineString.h:865
lanelet::BasicLineString3d
BasicPoints3d BasicLineString3d
Definition: primitives/LineString.h:15


lanelet2_core
Author(s): Fabian Poggenhans
autogenerated on Thu Mar 6 2025 03:25:52