ad_rss
Geometry.hpp
Go to the documentation of this file.
1 // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 //
3 // Copyright (C) 2020-2022 Intel Corporation
4 //
5 // SPDX-License-Identifier: LGPL-2.1-only
6 //
7 // ----------------- END LICENSE BLOCK -----------------------------------
8 
13 #pragma once
14 
15 #include <boost/geometry.hpp>
16 #include <boost/geometry/geometries/geometries.hpp>
17 #include <boost/geometry/geometries/point_xy.hpp>
18 #include <boost/geometry/geometries/polygon.hpp>
19 #include <sstream>
20 #include "ad/physics/AngleOperation.hpp"
21 #include "ad/physics/Distance.hpp"
24 
28 namespace ad {
32 namespace rss {
36 namespace unstructured {
37 
38 // Basic types
39 typedef boost::geometry::model::d2::point_xy<double> Point;
40 typedef boost::geometry::model::linestring<Point> Line;
41 typedef boost::geometry::model::polygon<Point, false> Polygon; // counterclockwise
42 typedef boost::geometry::model::multi_point<Point> MultiPoint;
43 
51 inline Point toPoint(ad::physics::Distance2D const &distance)
52 {
53  return Point(static_cast<double>(distance.x), static_cast<double>(distance.y));
54 }
55 
64 inline Point toPoint(ad::physics::Distance const &distanceX, ad::physics::Distance const &distanceY)
65 {
66  return Point(static_cast<double>(distanceX), static_cast<double>(distanceY));
67 }
68 
76 inline ad::physics::Distance2D toDistance(Point const &point)
77 {
78  ad::physics::Distance2D distance;
79  distance.x = ad::physics::Distance(point.x());
80  distance.y = ad::physics::Distance(point.y());
81  return distance;
82 }
83 
90 void toPolygon(world::UnstructuredTrajectorySet const &trajectorySet, Polygon &polygon);
91 
98 void toTrajectorySet(Polygon const &polygon, world::UnstructuredTrajectorySet &trajectorySet);
99 
108 bool isInsideHeadingRange(ad::physics::Angle const &angle, state::HeadingRange const &range);
109 
120  state::HeadingRange const &b,
121  std::vector<state::HeadingRange> &overlapRanges);
122 
131 bool getHeadingOverlap(state::HeadingRange const &headingRange, std::vector<state::HeadingRange> &overlapRanges);
132 
142 Point rotateAroundPoint(Point const &origin, Point const &relativePoint, ad::physics::Angle const &angle);
143 
153 Point getPointOnCircle(Point const &origin, ad::physics::Distance const &radius, ad::physics::Angle const &angle);
154 
164 Point getCircleOrigin(Point const &point, ad::physics::Distance const &radius, ad::physics::Angle const &angle);
165 
175 template <typename T>
176 void calculateCircleArc(Point origin,
177  ad::physics::Distance const &radius,
178  ad::physics::Angle const &from,
179  ad::physics::Angle const &delta,
180  ad::physics::Angle const &stepWidth,
181  T &geometry)
182 {
183  ad::physics::Angle currentAngle = from;
184  ad::physics::Angle maxAngle = currentAngle + delta;
185  while (currentAngle <= maxAngle)
186  {
187  boost::geometry::append(geometry, getPointOnCircle(origin, radius, currentAngle));
188  currentAngle += stepWidth;
189  }
190  if (currentAngle - stepWidth != maxAngle)
191  {
192  boost::geometry::append(geometry, getPointOnCircle(origin, radius, maxAngle));
193  }
194 }
195 
204 bool collides(world::UnstructuredTrajectorySet const &trajectorySet1,
205  world::UnstructuredTrajectorySet const &trajectorySet2);
206 
216 bool combinePolygon(Polygon const &a, Polygon const &b, Polygon &result);
217 
218 } // namespace unstructured
219 } // namespace rss
220 } // namespace ad
221 
230 inline ad::rss::unstructured::Point operator+(ad::rss::unstructured::Point const &a,
231  ad::rss::unstructured::Point const &b)
232 {
233  auto result = a;
234  boost::geometry::add_point(result, b);
235  return result;
236 }
237 
246 inline ad::rss::unstructured::Point operator-(ad::rss::unstructured::Point const &a,
247  ad::rss::unstructured::Point const &b)
248 {
249  auto result = a;
250  boost::geometry::subtract_point(result, b);
251  return result;
252 }
253 
262 inline bool operator==(ad::rss::unstructured::Point const &a, ad::rss::unstructured::Point const &b)
263 {
264  return (std::abs(a.x() - b.x()) <= std::numeric_limits<double>::epsilon())
265  && (std::abs(a.y() - b.y()) <= std::numeric_limits<double>::epsilon());
266 }
267 
276 inline bool operator!=(ad::rss::unstructured::Point const &a, ad::rss::unstructured::Point const &b)
277 {
278  return !(a == b);
279 }
280 
281 namespace std {
282 
290 inline std::string to_string(ad::rss::unstructured::Point const &value)
291 {
292  std::stringstream stream;
293  stream << "[" << value.x() << "," << value.y() << "]";
294  return stream.str();
295 }
296 
304 inline std::string to_string(ad::rss::unstructured::Polygon const &value)
305 {
306  std::stringstream stream;
307  stream << "[";
308  for (auto pt : value.outer())
309  {
310  stream << std::to_string(pt) << ",";
311  }
312  stream << "]";
313  return stream.str();
314 }
315 
323 inline std::string to_string(ad::rss::unstructured::Line value)
324 {
325  std::stringstream stream;
326  stream << "[";
327  for (auto pt : value)
328  {
329  stream << "[" << pt.x() << "," << pt.y() << "],";
330  }
331  stream << "]";
332  return stream.str();
333 }
334 } // namespace std
ad::rss::unstructured::getHeadingOverlap
bool getHeadingOverlap(state::HeadingRange const &a, state::HeadingRange const &b, std::vector< state::HeadingRange > &overlapRanges)
get the overlap between two angle ranges
ad::rss::unstructured::toTrajectorySet
void toTrajectorySet(Polygon const &polygon, world::UnstructuredTrajectorySet &trajectorySet)
convert a polygon to a trajectory set
ad
namespace ad
Definition: LateralRelativePosition.hpp:26
operator+
ad::rss::unstructured::Point operator+(ad::rss::unstructured::Point const &a, ad::rss::unstructured::Point const &b)
Point operation: vector addition.
Definition: Geometry.hpp:230
ad::rss::unstructured::getPointOnCircle
Point getPointOnCircle(Point const &origin, ad::physics::Distance const &radius, ad::physics::Angle const &angle)
calculate a point on a circle
ad::rss::unstructured::rotateAroundPoint
Point rotateAroundPoint(Point const &origin, Point const &relativePoint, ad::physics::Angle const &angle)
rotate a point around another point
ad::rss::unstructured::isInsideHeadingRange
bool isInsideHeadingRange(ad::physics::Angle const &angle, state::HeadingRange const &range)
check if an angle is within a range
ad::rss::unstructured::calculateCircleArc
void calculateCircleArc(Point origin, ad::physics::Distance const &radius, ad::physics::Angle const &from, ad::physics::Angle const &delta, ad::physics::Angle const &stepWidth, T &geometry)
calculate points on a circle arc
Definition: Geometry.hpp:176
ad::rss::unstructured::getCircleOrigin
Point getCircleOrigin(Point const &point, ad::physics::Distance const &radius, ad::physics::Angle const &angle)
calculate the circle origin
operator-
ad::rss::unstructured::Point operator-(ad::rss::unstructured::Point const &a, ad::rss::unstructured::Point const &b)
Point operation: vector subtraction.
Definition: Geometry.hpp:246
operator!=
bool operator!=(ad::rss::unstructured::Point const &a, ad::rss::unstructured::Point const &b)
comparison operation: Points not equal
Definition: Geometry.hpp:276
ad::rss::unstructured::combinePolygon
bool combinePolygon(Polygon const &a, Polygon const &b, Polygon &result)
Combine two polygons.
std::to_string
std::string to_string(::ad::rss::situation::LateralRelativePosition const &value)
overload of the std::to_string for LateralRelativePosition
Definition: LateralRelativePosition.hpp:160
ad::rss::unstructured::collides
bool collides(world::UnstructuredTrajectorySet const &trajectorySet1, world::UnstructuredTrajectorySet const &trajectorySet2)
check if two trajectory sets collide
UnstructuredTrajectorySet.hpp
ad::rss::world::UnstructuredTrajectorySet
::ad::physics::Distance2DList UnstructuredTrajectorySet
DataType UnstructuredTrajectorySet.
Definition: UnstructuredTrajectorySet.hpp:39
ad::rss::unstructured::toDistance
ad::physics::Distance2D toDistance(Point const &point)
convert a Point to a Distance2D
Definition: Geometry.hpp:76
HeadingRange.hpp
std::to_string
std::string to_string(ad::rss::unstructured::Line value)
to_string overload for Line
Definition: Geometry.hpp:323
ad::rss::unstructured::toPoint
Point toPoint(ad::physics::Distance2D const &distance)
create a Point from a Distance2D
Definition: Geometry.hpp:51
operator==
bool operator==(ad::rss::unstructured::Point const &a, ad::rss::unstructured::Point const &b)
comparison operation: Point
Definition: Geometry.hpp:262
ad::rss::unstructured::toPolygon
void toPolygon(world::UnstructuredTrajectorySet const &trajectorySet, Polygon &polygon)
convert a trajectory set to a polygon
ad::rss::state::HeadingRange
DataType HeadingRange.
Definition: HeadingRange.hpp:43