Autoware.Auto
common_2d.hpp
Go to the documentation of this file.
1 // Copyright 2017-2021 the Autoware Foundation
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 //    http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 //
15 // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
18 
19 #ifndef GEOMETRY__COMMON_2D_HPP_
20 #define GEOMETRY__COMMON_2D_HPP_
21 
22 #include <common/types.hpp>
23 #include <cmath>
24 #include <limits>
25 #include <stdexcept>
26 
27 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
28 #include "geometry/interval.hpp"
29 
32 
34 
35 namespace autoware
36 {
37 namespace common
38 {
39 namespace geometry
40 {
41 
43 namespace point_adapter
44 {
49 template<typename PointT>
50 inline auto x_(const PointT & pt)
51 {
52  return pt.x;
53 }
58 {
59  return pt.pose.position.x;
60 }
65 template<typename PointT>
66 inline auto y_(const PointT & pt)
67 {
68  return pt.y;
69 }
74 {
75  return pt.pose.position.y;
76 }
81 template<typename PointT>
82 inline auto z_(const PointT & pt)
83 {
84  return pt.z;
85 }
90 {
91  return pt.pose.position.z;
92 }
97 template<typename PointT>
98 inline auto & xr_(PointT & pt)
99 {
100  return pt.x;
101 }
106 {
107  return pt.pose.position.x;
108 }
113 template<typename PointT>
114 inline auto & yr_(PointT & pt)
115 {
116  return pt.y;
117 }
122 {
123  return pt.pose.position.y;
124 }
129 template<typename PointT>
130 inline auto & zr_(PointT & pt)
131 {
132  return pt.z;
133 }
138 {
139  return pt.pose.position.z;
140 }
141 } // namespace point_adapter
142 
143 namespace details
144 {
145 // Return the next iterator, cycling back to the beginning of the list if you hit the end
146 template<typename IT>
147 IT circular_next(const IT begin, const IT end, const IT current) noexcept
148 {
149  auto next = std::next(current);
150  if (end == next) {
151  next = begin;
152  }
153  return next;
154 }
155 
156 } // namespace details
157 
158 
165 template<typename T1, typename T2, typename T3>
166 inline auto ccw(const T1 & pt, const T2 & q, const T3 & r)
167 {
168  using point_adapter::x_;
169  using point_adapter::y_;
170 
171  return (((x_(q) - x_(pt)) * (y_(r) - y_(pt))) - ((y_(q) - y_(pt)) * (x_(r) - x_(pt)))) <= 0.0F;
172 }
173 
179 template<typename T1, typename T2>
180 inline auto cross_2d(const T1 & pt, const T2 & q)
181 {
182  using point_adapter::x_;
183  using point_adapter::y_;
184  return (x_(pt) * y_(q)) - (y_(pt) * x_(q));
185 }
186 
192 template<typename T1, typename T2>
193 inline auto dot_2d(const T1 & pt, const T2 & q)
194 {
195  using point_adapter::x_;
196  using point_adapter::y_;
197  return (x_(pt) * x_(q)) + (y_(pt) * y_(q));
198 }
199 
206 template<typename T>
207 T minus_2d(const T & p, const T & q)
208 {
209  T r;
210  using point_adapter::x_;
211  using point_adapter::y_;
212  point_adapter::xr_(r) = x_(p) - x_(q);
213  point_adapter::yr_(r) = y_(p) - y_(q);
214  return r;
215 }
216 
222 template<typename T>
223 T minus_2d(const T & p)
224 {
225  T r;
228  return r;
229 }
236 template<typename T>
237 T plus_2d(const T & p, const T & q)
238 {
239  T r;
240  using point_adapter::x_;
241  using point_adapter::y_;
242  point_adapter::xr_(r) = x_(p) + x_(q);
243  point_adapter::yr_(r) = y_(p) + y_(q);
244  return r;
245 }
246 
253 template<typename T>
254 T times_2d(const T & p, const float32_t a)
255 {
256  T r;
257  point_adapter::xr_(r) = static_cast<float32_t>(point_adapter::x_(p)) * a;
258  point_adapter::yr_(r) = static_cast<float32_t>(point_adapter::y_(p)) * a;
259  return r;
260 }
261 
272 template<typename T>
273 inline T intersection_2d(const T & pt, const T & u, const T & q, const T & v)
274 {
275  const float32_t num = cross_2d(minus_2d(pt, q), u);
276  float32_t den = cross_2d(v, u);
277  constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();
278  if (fabsf(den) < FEPS) {
279  if (fabsf(num) < FEPS) {
280  // collinear case, anything is ok
281  den = 1.0F;
282  } else {
283  // parallel case, no valid output
284  throw std::runtime_error(
285  "intersection_2d: no unique solution (either collinear or parallel)");
286  }
287  }
288  return plus_2d(q, times_2d(v, num / den));
289 }
290 
291 
297 template<typename T>
298 inline void rotate_2d(T & pt, const float32_t cos_th, const float32_t sin_th)
299 {
300  const float32_t x = point_adapter::x_(pt);
301  const float32_t y = point_adapter::y_(pt);
302  point_adapter::xr_(pt) = (cos_th * x) - (sin_th * y);
303  point_adapter::yr_(pt) = (sin_th * x) + (cos_th * y);
304 }
305 
311 template<typename T>
312 inline T rotate_2d(const T & pt, const float32_t th_rad)
313 {
314  T q(pt); // It's reasonable to expect a copy constructor
315  const float32_t s = sinf(th_rad);
316  const float32_t c = cosf(th_rad);
317  rotate_2d(q, c, s);
318  return q;
319 }
320 
326 template<typename T>
327 inline T get_normal(const T & pt)
328 {
329  T q(pt);
332  return q;
333 }
334 
339 template<typename T>
340 inline auto norm_2d(const T & pt)
341 {
342  return sqrtf(static_cast<float32_t>(dot_2d(pt, pt)));
343 }
344 
353 template<typename T>
354 inline T closest_segment_point_2d(const T & p, const T & q, const T & r)
355 {
356  const T qp = minus_2d(q, p);
357  const float32_t len2 = static_cast<float32_t>(dot_2d(qp, qp));
358  T ret = p;
359  if (len2 > std::numeric_limits<float32_t>::epsilon()) {
360  const Interval_f unit_interval(0.0f, 1.0f);
361  const float32_t val = static_cast<float32_t>(dot_2d(minus_2d(r, p), qp)) / len2;
362  const float32_t t = Interval_f::clamp_to(unit_interval, val);
363  ret = plus_2d(p, times_2d(qp, t));
364  }
365  return ret;
366 }
367 //
370 // Obtained by simplifying closest_segment_point_2d.
376 // define a line
377 template<typename T>
378 inline T closest_line_point_2d(const T & p, const T & q, const T & r)
379 {
380  const T qp = minus_2d(q, p);
381  const float32_t len2 = dot_2d(qp, qp);
382  T ret = p;
383  if (len2 > std::numeric_limits<float32_t>::epsilon()) {
384  const float32_t t = dot_2d(minus_2d(r, p), qp) / len2;
385  ret = plus_2d(p, times_2d(qp, t));
386  } else {
387  throw std::runtime_error(
388  "closet_line_point_2d: line ill-defined because given points coincide");
389  }
390  return ret;
391 }
392 
399 template<typename T>
400 inline auto point_line_segment_distance_2d(const T & p, const T & q, const T & r)
401 {
402  const T pq_r = minus_2d(closest_segment_point_2d(p, q, r), r);
403  return norm_2d(pq_r);
404 }
405 
410 template<typename T>
411 inline T make_unit_vector2d(float th)
412 {
413  T r;
414  point_adapter::xr_(r) = std::cos(th);
415  point_adapter::yr_(r) = std::sin(th);
416  return r;
417 }
418 
426 template<typename OUT = float32_t, typename T1, typename T2>
427 inline OUT squared_distance_2d(const T1 & a, const T2 & b)
428 {
429  const auto x = static_cast<OUT>(point_adapter::x_(a)) - static_cast<OUT>(point_adapter::x_(b));
430  const auto y = static_cast<OUT>(point_adapter::y_(a)) - static_cast<OUT>(point_adapter::y_(b));
431  return (x * x) + (y * y);
432 }
433 
441 template<typename OUT = float32_t, typename T1, typename T2>
442 inline OUT distance_2d(const T1 & a, const T2 & b)
443 {
444  return std::sqrt(squared_distance_2d<OUT>(a, b));
445 }
446 
456 template<typename T>
457 inline auto check_point_position_to_line_2d(const T & p1, const T & p2, const T & q)
458 {
459  return cross_2d(minus_2d(p2, p1), minus_2d(q, p1));
460 }
461 
469 template<typename IT>
470 bool all_ordered(const IT begin, const IT end) noexcept
471 {
472  // Short circuit: a line or point is always CCW or otherwise ill-defined
473  if (std::distance(begin, end) <= 2U) {
474  return true;
475  }
476  bool is_first_point_ccw = false;
477  // Can't use std::all_of because that works directly on the values
478  for (auto line_start = begin; line_start != end; ++line_start) {
479  const auto line_end = details::circular_next(begin, end, line_start);
480  const auto query_point = details::circular_next(begin, end, line_end);
481  // Check if 3 points starting from current point are in counter clockwise direction
482  const bool is_ccw = ccw(*line_start, *line_end, *query_point);
483  if (is_ccw) {
484  if (line_start == begin) {
485  is_first_point_ccw = true;
486  } else {
487  if (!is_first_point_ccw) {
488  return false;
489  }
490  }
491  } else if (is_first_point_ccw) {
492  return false;
493  }
494  }
495  return true;
496 }
497 
503 template<typename IT>
504 auto area_2d(const IT begin, const IT end) noexcept
505 {
506  using point_adapter::x_;
507  using point_adapter::y_;
508  using T = decltype(x_(*begin));
509  auto area = T{}; // zero initialization
510  // use the approach of https://www.mathwords.com/a/area_convex_polygon.htm
511  for (auto it = begin; it != end; ++it) {
512  const auto next = details::circular_next(begin, end, it);
513  area += x_(*it) * y_(*next);
514  area -= x_(*next) * y_(*it);
515  }
516  return std::abs(T{0.5} *area);
517 }
518 
526 template<typename IT>
527 auto area_checked_2d(const IT begin, const IT end)
528 {
529  if (!all_ordered(begin, end)) {
530  throw std::domain_error{"Cannot compute area: points are not ordered"};
531  }
532  return area_2d(begin, end);
533 }
534 
544 template<typename IteratorType, typename PointType>
546  const IteratorType & start_it, const IteratorType & end_it, const PointType & p)
547 {
548  std::int32_t winding_num = 0;
549 
550  for (IteratorType it = start_it; it != end_it; ++it) {
551  auto next_it = std::next(it);
552  if (next_it == end_it) {
553  next_it = start_it;
554  }
555 
558  {
559  return true;
560  }
561 
562  if (point_adapter::y_(*it) <= point_adapter::y_(p)) {
563  // Upward crossing edge
564  if (point_adapter::y_(*next_it) > point_adapter::y_(p)) {
565  if (comparison::abs_gt(
566  check_point_position_to_line_2d(*it, *next_it, p), 0.0F,
567  1e-3F))
568  {
569  ++winding_num;
570  } else {
572  check_point_position_to_line_2d(*it, *next_it, p),
573  1e-3F))
574  {
575  return true;
576  }
577  }
578  }
579  } else {
580  // Downward crossing edge
581  if (point_adapter::y_(*next_it) <= point_adapter::y_(p)) {
582  if (comparison::abs_lt(
583  check_point_position_to_line_2d(*it, *next_it, p), 0.0F,
584  1e-3F))
585  {
586  --winding_num;
587  } else {
589  check_point_position_to_line_2d(*it, *next_it, p),
590  1e-3F))
591  {
592  return true;
593  }
594  }
595  }
596  }
597  }
598 
599  return winding_num != 0;
600 }
601 
602 
603 } // namespace geometry
604 } // namespace common
605 } // namespace autoware
606 
607 #endif // GEOMETRY__COMMON_2D_HPP_
motion::motion_testing_nodes::TrajectoryPoint
autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint
Definition: motion_testing_publisher.hpp:36
autoware::common::geometry::times_2d
T times_2d(const T &p, const float32_t a)
The scalar multiplication operation, p * a.
Definition: common_2d.hpp:254
autoware::common::helper_functions::comparisons::abs_gt
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
autoware::common::geometry::check_point_position_to_line_2d
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
autoware::common::geometry::get_normal
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
autoware::common::geometry::cross_2d
auto cross_2d(const T1 &pt, const T2 &q)
compute p x q = p1 * q2 - p2 * q1
Definition: common_2d.hpp:180
autoware::common::geometry::point_adapter::x_
auto x_(const PointT &pt)
Gets the x value for a point.
Definition: common_2d.hpp:50
autoware::common::helper_functions::comparisons::abs_lt
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
types.hpp
This file includes common type definition.
y
DifferentialState y
Definition: kinematic_bicycle.snippet.hpp:28
u
DifferentialState u
Definition: kinematic_bicycle.snippet.hpp:29
autoware::common::geometry::closest_line_point_2d
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
autoware::common::geometry::Interval::clamp_to
static T clamp_to(const Interval &i, T val)
Clamp a scalar 'val' onto 'interval'.
Definition: interval.hpp:348
autoware::common::geometry::squared_distance_2d
OUT squared_distance_2d(const T1 &a, const T2 &b)
Compute squared euclidean distance between two points.
Definition: common_2d.hpp:427
autoware::common::geometry::area_2d
auto area_2d(const IT begin, const IT end) noexcept
Definition: common_2d.hpp:504
autoware::common::geometry::dot_2d
auto dot_2d(const T1 &pt, const T2 &q)
compute p * q = p1 * q1 + p2 * q2
Definition: common_2d.hpp:193
autoware::common::geometry::all_ordered
bool all_ordered(const IT begin, const IT end) noexcept
Definition: common_2d.hpp:470
autoware::common::geometry::is_point_inside_polygon_2d
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
autoware::common::geometry::plus_2d
T plus_2d(const T &p, const T &q)
The 2d addition operation, p + q.
Definition: common_2d.hpp:237
f
DifferentialEquation f
Definition: kinematic_bicycle.snippet.hpp:44
autoware::common::geometry::point_adapter::x_
auto x_(const autoware_auto_planning_msgs::msg::TrajectoryPoint &pt)
Gets the x value for a TrajectoryPoint message.
Definition: common_2d.hpp:57
autoware::common::geometry::details::circular_next
IT circular_next(const IT begin, const IT end, const IT current) noexcept
Definition: common_2d.hpp:147
autoware::common::types::bool8_t
bool bool8_t
Definition: types.hpp:39
autoware::common::geometry::area_checked_2d
auto area_checked_2d(const IT begin, const IT end)
Definition: common_2d.hpp:527
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
autoware::common::geometry::point_line_segment_distance_2d
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
autoware::common::geometry::point_adapter::yr_
auto & yr_(PointT &pt)
Gets a reference to the y value for a point.
Definition: common_2d.hpp:114
autoware::common::geometry::norm_2d
auto norm_2d(const T &pt)
get magnitude of x and y components:
Definition: common_2d.hpp:340
x
DifferentialState x
Definition: kinematic_bicycle.snippet.hpp:28
autoware::common::geometry::closest_segment_point_2d
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
autoware::common::helper_functions::comparisons
Definition: bool_comparisons.hpp:32
autoware::common::types::float32_t
float float32_t
Definition: types.hpp:45
autoware::common::geometry::point_adapter::xr_
auto & xr_(PointT &pt)
Gets a reference to the x value for a point.
Definition: common_2d.hpp:98
autoware::common::helper_functions::comparisons::abs_eq
bool abs_eq(const T &a, const T &b, const T &eps)
Check for approximate equality in absolute terms.
Definition: float_comparisons.hpp:43
autoware::common::helper_functions::comparisons::abs_eq_zero
bool abs_eq_zero(const T &a, const T &eps)
Check whether a value is within epsilon of zero.
Definition: float_comparisons.hpp:102
interval.hpp
autoware::common::geometry::minus_2d
T minus_2d(const T &p, const T &q)
Compute the 2d difference between two points, p - q.
Definition: common_2d.hpp:207
autoware::common::geometry::rotate_2d
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
autoware::common::geometry::intersection_2d
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
autoware::common::geometry::point_adapter::y_
auto y_(const autoware_auto_planning_msgs::msg::TrajectoryPoint &pt)
Gets the y value for a TrajectoryPoint message.
Definition: common_2d.hpp:73
autoware::common::geometry::make_unit_vector2d
T make_unit_vector2d(float th)
Make a 2D unit vector given an angle.
Definition: common_2d.hpp:411
autoware::common::geometry::point_adapter::y_
auto y_(const PointT &pt)
Gets the y value for a point.
Definition: common_2d.hpp:66
autoware::common::geometry::point_adapter::z_
auto z_(const PointT &pt)
Gets the z value for a point.
Definition: common_2d.hpp:82
autoware::common::geometry::ccw
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
autoware::common::geometry::point_adapter::zr_
auto & zr_(PointT &pt)
Gets a reference to the z value for a point.
Definition: common_2d.hpp:130
autoware::common::geometry::distance_2d
OUT distance_2d(const T1 &a, const T2 &b)
Compute euclidean distance between two points.
Definition: common_2d.hpp:442
autoware::common::geometry::Interval
Data structure to contain scalar interval bounds.
Definition: interval.hpp:52
v
DifferentialState v
Definition: linear_tire.snippet.hpp:28
get_open_port.end
end
Definition: scripts/get_open_port.py:23