Go to the documentation of this file.
19 #ifndef GEOMETRY__COMMON_2D_HPP_
20 #define GEOMETRY__COMMON_2D_HPP_
27 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
43 namespace point_adapter
49 template<
typename Po
intT>
50 inline auto x_(
const PointT & pt)
59 return pt.pose.position.x;
65 template<
typename Po
intT>
66 inline auto y_(
const PointT & pt)
75 return pt.pose.position.y;
81 template<
typename Po
intT>
82 inline auto z_(
const PointT & pt)
91 return pt.pose.position.z;
97 template<
typename Po
intT>
98 inline auto &
xr_(PointT & pt)
107 return pt.pose.position.x;
113 template<
typename Po
intT>
114 inline auto &
yr_(PointT & pt)
123 return pt.pose.position.y;
129 template<
typename Po
intT>
130 inline auto &
zr_(PointT & pt)
139 return pt.pose.position.z;
146 template<
typename IT>
149 auto next = std::next(current);
165 template<
typename T1,
typename T2,
typename T3>
166 inline auto ccw(
const T1 & pt,
const T2 & q,
const T3 & r)
171 return (((
x_(q) -
x_(pt)) * (
y_(r) -
y_(pt))) - ((
y_(q) -
y_(pt)) * (
x_(r) -
x_(pt)))) <= 0.0F;
179 template<
typename T1,
typename T2>
184 return (
x_(pt) *
y_(q)) - (
y_(pt) *
x_(q));
192 template<
typename T1,
typename T2>
193 inline auto dot_2d(
const T1 & pt,
const T2 & q)
197 return (
x_(pt) *
x_(q)) + (
y_(pt) *
y_(q));
277 constexpr
auto FEPS = std::numeric_limits<float32_t>::epsilon();
278 if (fabsf(den) < FEPS) {
279 if (fabsf(num) < FEPS) {
284 throw std::runtime_error(
285 "intersection_2d: no unique solution (either collinear or parallel)");
359 if (len2 > std::numeric_limits<float32_t>::epsilon()) {
383 if (len2 > std::numeric_limits<float32_t>::epsilon()) {
387 throw std::runtime_error(
388 "closet_line_point_2d: line ill-defined because given points coincide");
426 template<
typename OUT =
float32_t,
typename T1,
typename T2>
431 return (
x *
x) + (
y *
y);
441 template<
typename OUT =
float32_t,
typename T1,
typename T2>
444 return std::sqrt(squared_distance_2d<OUT>(a, b));
469 template<
typename IT>
473 if (std::distance(begin,
end) <= 2U) {
476 bool is_first_point_ccw =
false;
478 for (
auto line_start = begin; line_start !=
end; ++line_start) {
482 const bool is_ccw =
ccw(*line_start, *line_end, *query_point);
484 if (line_start == begin) {
485 is_first_point_ccw =
true;
487 if (!is_first_point_ccw) {
491 }
else if (is_first_point_ccw) {
503 template<
typename IT>
508 using T = decltype(
x_(*begin));
511 for (
auto it = begin; it !=
end; ++it) {
513 area +=
x_(*it) *
y_(*next);
514 area -=
x_(*next) *
y_(*it);
516 return std::abs(T{0.5} *area);
526 template<
typename IT>
530 throw std::domain_error{
"Cannot compute area: points are not ordered"};
544 template<
typename IteratorType,
typename Po
intType>
546 const IteratorType & start_it,
const IteratorType & end_it,
const PointType & p)
548 std::int32_t winding_num = 0;
550 for (IteratorType it = start_it; it != end_it; ++it) {
551 auto next_it = std::next(it);
552 if (next_it == end_it) {
599 return winding_num != 0;
607 #endif // GEOMETRY__COMMON_2D_HPP_
autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint
Definition: motion_testing_publisher.hpp:36
T times_2d(const T &p, const float32_t a)
The scalar multiplication operation, p * a.
Definition: common_2d.hpp:254
bool abs_gt(const T &a, const T &b, const T &eps)
Check for approximate greater than in absolute terms.
Definition: float_comparisons.hpp:91
auto check_point_position_to_line_2d(const T &p1, const T &p2, const T &q)
Check the given point's position relative the infinite line passing from p1 to p2....
Definition: common_2d.hpp:457
T get_normal(const T &pt)
compute q s.t. p T q, or p * q = 0 This is the equivalent of a 90 degree ccw rotation
Definition: common_2d.hpp:327
auto cross_2d(const T1 &pt, const T2 &q)
compute p x q = p1 * q2 - p2 * q1
Definition: common_2d.hpp:180
auto x_(const PointT &pt)
Gets the x value for a point.
Definition: common_2d.hpp:50
bool abs_lt(const T &a, const T &b, const T &eps)
Check for approximate less than in absolute terms.
Definition: float_comparisons.hpp:58
This file includes common type definition.
DifferentialState y
Definition: kinematic_bicycle.snippet.hpp:28
DifferentialState u
Definition: kinematic_bicycle.snippet.hpp:29
T closest_line_point_2d(const T &p, const T &q, const T &r)
Compute the closest point on the line going through p-q to point r.
Definition: common_2d.hpp:378
static T clamp_to(const Interval &i, T val)
Clamp a scalar 'val' onto 'interval'.
Definition: interval.hpp:348
OUT squared_distance_2d(const T1 &a, const T2 &b)
Compute squared euclidean distance between two points.
Definition: common_2d.hpp:427
auto area_2d(const IT begin, const IT end) noexcept
Definition: common_2d.hpp:504
auto dot_2d(const T1 &pt, const T2 &q)
compute p * q = p1 * q1 + p2 * q2
Definition: common_2d.hpp:193
bool all_ordered(const IT begin, const IT end) noexcept
Definition: common_2d.hpp:470
bool is_point_inside_polygon_2d(const IteratorType &start_it, const IteratorType &end_it, const PointType &p)
Check if the given point is inside or on the edge of the given polygon.
Definition: common_2d.hpp:545
T plus_2d(const T &p, const T &q)
The 2d addition operation, p + q.
Definition: common_2d.hpp:237
DifferentialEquation f
Definition: kinematic_bicycle.snippet.hpp:44
auto x_(const autoware_auto_planning_msgs::msg::TrajectoryPoint &pt)
Gets the x value for a TrajectoryPoint message.
Definition: common_2d.hpp:57
IT circular_next(const IT begin, const IT end, const IT current) noexcept
Definition: common_2d.hpp:147
bool bool8_t
Definition: types.hpp:39
auto area_checked_2d(const IT begin, const IT end)
Definition: common_2d.hpp:527
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
auto point_line_segment_distance_2d(const T &p, const T &q, const T &r)
Compute the distance from line segment p-q to point r.
Definition: common_2d.hpp:400
auto & yr_(PointT &pt)
Gets a reference to the y value for a point.
Definition: common_2d.hpp:114
auto norm_2d(const T &pt)
get magnitude of x and y components:
Definition: common_2d.hpp:340
DifferentialState x
Definition: kinematic_bicycle.snippet.hpp:28
T closest_segment_point_2d(const T &p, const T &q, const T &r)
Compute the closest point on line segment p-q to point r Based on equations from https://stackoverflo...
Definition: common_2d.hpp:354
Definition: bool_comparisons.hpp:32
float float32_t
Definition: types.hpp:45
auto & xr_(PointT &pt)
Gets a reference to the x value for a point.
Definition: common_2d.hpp:98
bool abs_eq(const T &a, const T &b, const T &eps)
Check for approximate equality in absolute terms.
Definition: float_comparisons.hpp:43
bool abs_eq_zero(const T &a, const T &eps)
Check whether a value is within epsilon of zero.
Definition: float_comparisons.hpp:102
T minus_2d(const T &p, const T &q)
Compute the 2d difference between two points, p - q.
Definition: common_2d.hpp:207
void rotate_2d(T &pt, const float32_t cos_th, const float32_t sin_th)
rotate point given precomputed sin and cos
Definition: common_2d.hpp:298
T intersection_2d(const T &pt, const T &u, const T &q, const T &v)
solve p + t * u = q + s * v Ref: https://stackoverflow.com/questions/563198/ whats-the-most-efficent-...
Definition: common_2d.hpp:273
auto y_(const autoware_auto_planning_msgs::msg::TrajectoryPoint &pt)
Gets the y value for a TrajectoryPoint message.
Definition: common_2d.hpp:73
T make_unit_vector2d(float th)
Make a 2D unit vector given an angle.
Definition: common_2d.hpp:411
auto y_(const PointT &pt)
Gets the y value for a point.
Definition: common_2d.hpp:66
auto z_(const PointT &pt)
Gets the z value for a point.
Definition: common_2d.hpp:82
auto ccw(const T1 &pt, const T2 &q, const T3 &r)
compute whether line segment rp is counter clockwise relative to line segment qp
Definition: common_2d.hpp:166
auto & zr_(PointT &pt)
Gets a reference to the z value for a point.
Definition: common_2d.hpp:130
OUT distance_2d(const T1 &a, const T2 &b)
Compute euclidean distance between two points.
Definition: common_2d.hpp:442
Data structure to contain scalar interval bounds.
Definition: interval.hpp:52
DifferentialState v
Definition: linear_tire.snippet.hpp:28
end
Definition: scripts/get_open_port.py:23