Branch data Line data Source code
1 : : // Copyright 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 : :
17 : : #include <geometry/spatial_hash.hpp>
18 : : #include <geometry_msgs/msg/point32.hpp>
19 : : //lint -e537 NOLINT repeated include file due to cpplint rule
20 : : #include <algorithm>
21 : : //lint -e537 NOLINT repeated include file due to cpplint rule
22 : : #include <limits>
23 : :
24 : : namespace autoware
25 : : {
26 : : namespace common
27 : : {
28 : : namespace geometry
29 : : {
30 : : namespace spatial_hash
31 : : {
32 : : ////////////////////////////////////////////////////////////////////////////////
33 : 36 : Config2d::Config2d(
34 : : const float32_t min_x,
35 : : const float32_t max_x,
36 : : const float32_t min_y,
37 : : const float32_t max_y,
38 : : const float32_t radius,
39 : 36 : const Index capacity)
40 : : : Config(min_x, max_x, min_y, max_y, {}, std::numeric_limits<float32_t>::min(),
41 : 36 : radius, capacity)
42 : : {
43 : 32 : }
44 : : ////////////////////////////////////////////////////////////////////////////////
45 : 669 : Index Config2d::bin_(const float32_t x, const float32_t y, const float32_t z) const
46 : : {
47 : : (void)z;
48 : 669 : return bin_impl(x, y);
49 : : }
50 : : ////////////////////////////////////////////////////////////////////////////////
51 : 5842 : bool Config2d::valid(
52 : : const details::Index3 & ref,
53 : : const details::Index3 & query,
54 : : const float ref_distance2) const
55 : : {
56 [ + + ]: 5842 : const float dx = idx_distance(ref.x, query.x);
57 [ + + ]: 5842 : const float dy = idx_distance(ref.y, query.y);
58 : : // minor algebraic manipulation happening below
59 : 5842 : return (((dx * dx) + (dy * dy)) * side_length2()) <= ref_distance2;
60 : : }
61 : : ////////////////////////////////////////////////////////////////////////////////
62 : 650 : details::Index3 Config2d::index3_(const float32_t x, const float32_t y, const float32_t z) const
63 : : {
64 : : (void)z;
65 : 650 : return {x_index(x), y_index(y), Index{}}; // zero initialization
66 : : }
67 : : ////////////////////////////////////////////////////////////////////////////////
68 : 5842 : Index Config2d::index_(const details::Index3 & idx) const
69 : : {
70 : 5842 : return bin_impl(idx.x, idx.y);
71 : : }
72 : : ////////////////////////////////////////////////////////////////////////////////
73 : 4 : Config3d::Config3d(
74 : : const float32_t min_x,
75 : : const float32_t max_x,
76 : : const float32_t min_y,
77 : : const float32_t max_y,
78 : : const float32_t min_z,
79 : : const float32_t max_z,
80 : : const float32_t radius,
81 : 4 : const Index capacity)
82 : 4 : : Config(min_x, max_x, min_y, max_y, min_z, max_z, radius, capacity)
83 : : {
84 : 1 : }
85 : : ////////////////////////////////////////////////////////////////////////////////
86 : 160 : Index Config3d::bin_(const float32_t x, const float32_t y, const float32_t z) const
87 : : {
88 : 160 : return bin_impl(x, y, z);
89 : : }
90 : : ////////////////////////////////////////////////////////////////////////////////
91 : 27 : bool Config3d::valid(
92 : : const details::Index3 & ref,
93 : : const details::Index3 & query,
94 : : const float ref_distance2) const
95 : : {
96 [ + + ]: 27 : const float dx = idx_distance(ref.x, query.x);
97 [ + + ]: 27 : const float dy = idx_distance(ref.y, query.y);
98 [ + + ]: 27 : const float dz = idx_distance(ref.z, query.z);
99 : : // minor algebraic manipulation happening below
100 : 27 : return (((dx * dx) + (dy * dy) + (dz * dz)) * side_length2()) <= ref_distance2;
101 : : }
102 : : ////////////////////////////////////////////////////////////////////////////////
103 : 1 : details::Index3 Config3d::index3_(const float32_t x, const float32_t y, const float32_t z) const
104 : : {
105 : 1 : return {x_index(x), y_index(y), z_index(z)}; // zero initialization
106 : : }
107 : : ////////////////////////////////////////////////////////////////////////////////
108 : 27 : Index Config3d::index_(const details::Index3 & idx) const
109 : : {
110 : 27 : return bin_impl(idx.x, idx.y, idx.z);
111 : : }
112 : : ////////////////////////////////////////////////////////////////////////////////
113 : : template class SpatialHash<geometry_msgs::msg::Point32, Config2d>;
114 : : template class SpatialHash<geometry_msgs::msg::Point32, Config3d>;
115 : : } // namespace spatial_hash
116 : : } // namespace geometry
117 : : } // namespace common
118 : : } // namespace autoware
|
