Branch data Line data Source code
1 : : // Copyright 2017-2019 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.
16 : : /// \file
17 : : /// \brief This file includes common functionality for 2D geometry, such as dot products
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 "geometry/interval.hpp"
28 : :
29 : : using autoware::common::types::float32_t;
30 : : using autoware::common::types::bool8_t;
31 : :
32 : : namespace autoware
33 : : {
34 : : namespace common
35 : : {
36 : : namespace geometry
37 : : {
38 : :
39 : : /// \brief Temporary namespace for point adapter methods, for use with nonstandard point types
40 : : namespace point_adapter
41 : : {
42 : : /// \brief Gets the x value for a point
43 : : /// \return The x value of the point
44 : : /// \param[in] pt The point
45 : : /// \tparam PointT The point type
46 : : template<typename PointT>
47 : 259203 : inline auto x_(const PointT & pt)
48 : : {
49 : 259203 : return pt.x;
50 : : }
51 : : /// \brief Gets the y value for a point
52 : : /// \return The y value of the point
53 : : /// \param[in] pt The point
54 : : /// \tparam PointT The point type
55 : : template<typename PointT>
56 : 191991 : inline auto y_(const PointT & pt)
57 : : {
58 : 191991 : return pt.y;
59 : : }
60 : : /// \brief Gets the z value for a point
61 : : /// \return The z value of the point
62 : : /// \param[in] pt The point
63 : : /// \tparam PointT The point type
64 : : template<typename PointT>
65 : 343 : inline auto z_(const PointT & pt)
66 : : {
67 : 343 : return pt.z;
68 : : }
69 : : /// \brief Gets a reference to the x value for a point
70 : : /// \return A reference to the x value of the point
71 : : /// \param[in] pt The point
72 : : /// \tparam PointT The point type
73 : : template<typename PointT>
74 : 49934 : inline auto & xr_(PointT & pt)
75 : : {
76 : 49934 : return pt.x;
77 : : }
78 : : /// \brief Gets a reference to the y value for a point
79 : : /// \return A reference to The y value of the point
80 : : /// \param[in] pt The point
81 : : /// \tparam PointT The point type
82 : : template<typename PointT>
83 : 49934 : inline auto & yr_(PointT & pt)
84 : : {
85 : 49934 : return pt.y;
86 : : }
87 : : /// \brief Gets a reference to the z value for a point
88 : : /// \return A reference to the z value of the point
89 : : /// \param[in] pt The point
90 : : /// \tparam PointT The point type
91 : : template<typename PointT>
92 : : inline auto & zr_(PointT & pt)
93 : : {
94 : : return pt.z;
95 : : }
96 : : } // namespace point_adapter
97 : :
98 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
99 : : /// \brief compute whether line segment rp is counter clockwise relative to line segment qp
100 : : /// \param[in] pt shared point for both line segments
101 : : /// \param[in] r point to check if it forms a ccw angle
102 : : /// \param[in] q reference point
103 : : /// \return whether angle formed is ccw. Three collinear points is considered ccw
104 : : template<typename T>
105 : 4831 : inline bool8_t ccw(const T & pt, const T & q, const T & r)
106 : : {
107 : : using point_adapter::x_;
108 : : using point_adapter::y_;
109 [ # # # # : 4831 : return (((x_(q) - x_(pt)) * (y_(r) - y_(pt))) - ((y_(q) - y_(pt)) * (x_(r) - x_(pt)))) <= 0.0F;
# # # # ]
110 : : }
111 : :
112 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
113 : : /// \brief compute p x q = p1 * q2 - p2 * q1
114 : : /// \param[in] pt first point
115 : : /// \param[in] q second point
116 : : /// \return 2d cross product
117 : : template<typename T>
118 : 22248 : inline float32_t cross_2d(const T & pt, const T & q)
119 : : {
120 : : using point_adapter::x_;
121 : : using point_adapter::y_;
122 [ # # # # ]: 22248 : return (x_(pt) * y_(q)) - (y_(pt) * x_(q));
123 : : }
124 : :
125 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
126 : : /// \brief compute p * q = p1 * q1 + p2 * q2
127 : : /// \param[in] pt first point
128 : : /// \param[in] q second point
129 : : /// \return 2d scalar dot product
130 : : template<typename T>
131 : 28782 : inline float32_t dot_2d(const T & pt, const T & q)
132 : : {
133 : : using point_adapter::x_;
134 : : using point_adapter::y_;
135 [ # # # # : 28782 : return (x_(pt) * x_(q)) + (y_(pt) * y_(q));
# # # # #
# # # # #
# # ]
136 : : }
137 : :
138 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
139 : : /// \brief Compute the 2d difference between two points, p - q
140 : : /// \param[in] p The left hand side
141 : : /// \param[in] q The right hand side
142 : : /// \return A point with the difference in the x and y fields, all other fields are default
143 : : /// initialized
144 : : template<typename T>
145 : 11470 : T minus_2d(const T & p, const T & q)
146 : : {
147 : 11470 : T r;
148 : : using point_adapter::x_;
149 : : using point_adapter::y_;
150 : 22508 : point_adapter::xr_(r) = x_(p) - x_(q);
151 : 11470 : point_adapter::yr_(r) = y_(p) - y_(q);
152 : 11470 : return r;
153 : : }
154 : :
155 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
156 : : /// \brief The unary minus or negation operator applied to a single point's 2d fields
157 : : /// \param[in] p The left hand side
158 : : /// \return A point with the negation in the x and y fields, all other fields are default
159 : : /// initialized
160 : : template<typename T>
161 : 108 : T minus_2d(const T & p)
162 : : {
163 : 108 : T r;
164 : 108 : point_adapter::xr_(r) = -point_adapter::x_(p);
165 : 108 : point_adapter::yr_(r) = -point_adapter::y_(p);
166 : 108 : return r;
167 : : }
168 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
169 : : /// \brief The 2d addition operation, p + q
170 : : /// \param[in] p The left hand side
171 : : /// \param[in] q The right hand side
172 : : /// \return A point with the sum in the x and y fields, all other fields are default
173 : : /// initialized
174 : : template<typename T>
175 : 8948 : T plus_2d(const T & p, const T & q)
176 : : {
177 : 8948 : T r;
178 : : using point_adapter::x_;
179 : : using point_adapter::y_;
180 : 9659 : point_adapter::xr_(r) = x_(p) + x_(q);
181 : 9659 : point_adapter::yr_(r) = y_(p) + y_(q);
182 : 8948 : return r;
183 : : }
184 : :
185 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
186 : : /// \brief The scalar multiplication operation, p * a
187 : : /// \param[in] p The point value
188 : : /// \param[in] a The scalar value
189 : : /// \return A point with the scaled x and y fields, all other fields are default
190 : : /// initialized
191 : : template<typename T>
192 : 8960 : T times_2d(const T & p, const float32_t a)
193 : : {
194 : 8960 : T r;
195 : 9671 : point_adapter::xr_(r) = point_adapter::x_(p) * a;
196 : 8960 : point_adapter::yr_(r) = point_adapter::y_(p) * a;
197 : 8960 : return r;
198 : : }
199 : :
200 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
201 : : /// \brief solve p + t * u = q + s * v
202 : : /// Ref: https://stackoverflow.com/questions/563198/
203 : : /// whats-the-most-efficent-way-to-calculate-where-two-line-segments-intersect
204 : : /// \param[in] pt anchor point of first line
205 : : /// \param[in] u direction of first line
206 : : /// \param[in] q anchor point of second line
207 : : /// \param[in] v direction of second line
208 : : /// \return intersection point
209 : : /// \throw std::runtime_error if lines are (nearly) collinear or parallel
210 : : template<typename T>
211 : 8936 : inline T intersection_2d(const T & pt, const T & u, const T & q, const T & v)
212 : : {
213 : 8936 : const float32_t num = cross_2d(minus_2d(pt, q), u);
214 : 8936 : float32_t den = cross_2d(v, u);
215 : 8936 : constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();
216 [ - + ]: 8936 : if (fabsf(den) < FEPS) {
217 [ # # ]: 0 : if (fabsf(num) < FEPS) {
218 : : // collinear case, anything is ok
219 : 0 : den = 1.0F;
220 : : } else {
221 : : // parallel case, no valid output
222 : : throw std::runtime_error(
223 [ # # ]: 0 : "intersection_2d: no unique solution (either collinear or parallel)");
224 : : }
225 : : }
226 : 8936 : return plus_2d(q, times_2d(v, num / den));
227 : : }
228 : :
229 : :
230 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
231 : : /// \brief rotate point given precomputed sin and cos
232 : : /// \param[inout] pt point to rotate
233 : : /// \param[in] cos_th precomputed cosine value
234 : : /// \param[in] sin_th precompined sine value
235 : : template<typename T>
236 : 8720 : inline void rotate_2d(T & pt, const float32_t cos_th, const float32_t sin_th)
237 : : {
238 : 8720 : const float32_t x = point_adapter::x_(pt);
239 : 8720 : const float32_t y = point_adapter::y_(pt);
240 : 8720 : point_adapter::xr_(pt) = (cos_th * x) - (sin_th * y);
241 : 8720 : point_adapter::yr_(pt) = (sin_th * x) + (cos_th * y);
242 : 8720 : }
243 : :
244 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
245 : : /// \brief rotate by radian angle th in z direction with ccw positive
246 : : /// \param[in] pt reference point to rotate
247 : : /// \param[in] th_rad angle by which to rotate point
248 : : /// \return rotated point
249 : : template<typename T>
250 : : inline T rotate_2d(const T & pt, const float32_t th_rad)
251 : : {
252 : : T q(pt); // It's reasonable to expect a copy constructor
253 : : const float32_t s = sinf(th_rad);
254 : : const float32_t c = cosf(th_rad);
255 : : rotate_2d(q, c, s);
256 : : return q;
257 : : }
258 : :
259 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
260 : : /// \brief compute q s.t. p T q, or p * q = 0
261 : : /// This is the equivalent of a 90 degree ccw rotation
262 : : /// \param[in] pt point to get normal point of
263 : : /// \return point normal to p (unnormalized)
264 : : template<typename T>
265 : 50 : inline T get_normal(const T & pt)
266 : : {
267 : 50 : T q(pt);
268 : 50 : point_adapter::xr_(q) = -point_adapter::y_(pt);
269 : 50 : point_adapter::yr_(q) = point_adapter::x_(pt);
270 : 50 : return q;
271 : : }
272 : :
273 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
274 : : /// \brief get magnitude of x and y components:
275 : : /// \param[in] pt point to get magnitude of
276 : : /// \return magitude of x and y components together
277 : : template<typename T>
278 : 17548 : inline float32_t norm_2d(const T & pt)
279 : : {
280 [ - - - - : 23083 : return sqrtf(dot_2d(pt, pt));
- - - - +
+ + + ]
281 : : }
282 : :
283 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
284 : : /// \brief Compute the closest point on line segment p-q to point r
285 : : /// Based on equations from https://stackoverflow.com/a/1501725 and
286 : : /// http://paulbourke.net/geometry/pointlineplane/
287 : : /// \param[in] p First point defining the line segment
288 : : /// \param[in] q Second point defining the line segment
289 : : /// \param[in] r Reference point to find the closest point to
290 : : /// \return Closest point on line segment p-q to point r
291 : : template<typename T>
292 : 12 : inline T closest_segment_point_2d(const T & p, const T & q, const T & r)
293 : : {
294 : 12 : const T qp = minus_2d(q, p);
295 : 12 : const float32_t len2 = dot_2d(qp, qp);
296 : 12 : T ret = p;
297 [ + + ]: 12 : if (len2 > std::numeric_limits<float32_t>::epsilon()) {
298 [ + - ]: 8 : const Interval_f unit_interval(0.0f, 1.0f);
299 : 8 : const float32_t val = dot_2d(minus_2d(r, p), qp) / len2;
300 [ + - ]: 8 : const float32_t t = Interval_f::clamp_to(unit_interval, val);
301 : 8 : ret = plus_2d(p, times_2d(qp, t));
302 : : }
303 : 18 : return ret;
304 : : }
305 : : //
306 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
307 : : /// \brief Compute the closest point on the line going through p-q to point r
308 : : // Obtained by simplifying closest_segment_point_2d.
309 : : /// \param[in] p First point defining the line
310 : : /// \param[in] q Second point defining the line
311 : : /// \param[in] r Reference point to find the closest point to
312 : : /// \return Closest point on line p-q to point r
313 : : /// \throw std::runtime_error if the two points coincide and hence don't uniquely
314 : : // define a line
315 : : template<typename T>
316 : 6 : inline T closest_line_point_2d(const T & p, const T & q, const T & r)
317 : : {
318 : 6 : const T qp = minus_2d(q, p);
319 : 6 : const float32_t len2 = dot_2d(qp, qp);
320 : 6 : T ret = p;
321 [ + + ]: 6 : if (len2 > std::numeric_limits<float32_t>::epsilon()) {
322 : 4 : const float32_t t = dot_2d(minus_2d(r, p), qp) / len2;
323 : 4 : ret = plus_2d(p, times_2d(qp, t));
324 : : } else {
325 : : throw std::runtime_error(
326 [ + - ]: 2 : "closet_line_point_2d: line ill-defined because given points coincide");
327 : : }
328 : 6 : return ret;
329 : : }
330 : :
331 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
332 : : /// \brief Compute the distance from line segment p-q to point r
333 : : /// \param[in] p First point defining the line segment
334 : : /// \param[in] q Second point defining the line segment
335 : : /// \param[in] r Reference point to find the distance from the line segment to
336 : : /// \return Distance from point r to line segment p-q
337 : : template<typename T>
338 : 6 : inline float32_t point_line_segment_distance_2d(const T & p, const T & q, const T & r)
339 : : {
340 [ + - ]: 6 : const T pq_r = minus_2d(closest_segment_point_2d(p, q, r), r);
341 : 12 : return norm_2d(pq_r);
342 : : }
343 : :
344 : : /// \brief Make a 2D unit vector given an angle.
345 : : /// \tparam T Point type. Must have point adapters defined or have float members x and y
346 : : /// \param th Angle in radians
347 : : /// \return Unit vector in the direction of the given angle.
348 : : template<typename T>
349 : : inline T make_unit_vector2d(float th)
350 : : {
351 : : T r;
352 : : point_adapter::xr_(r) = std::cos(th);
353 : : point_adapter::yr_(r) = std::sin(th);
354 : : return r;
355 : : }
356 : :
357 : : } // namespace geometry
358 : : } // namespace common
359 : : } // namespace autoware
360 : :
361 : : #endif // GEOMETRY__COMMON_2D_HPP_
|
