GCC Code Coverage Report

Directory: ./
File: include/coal/BV/BV.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 57 86 66.3%
Branches: 44 162 27.2%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2015, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 /** \author Jia Pan */
37
38 #ifndef COAL_BV_H
39 #define COAL_BV_H
40
41 #include "coal/BV/kDOP.h"
42 #include "coal/BV/AABB.h"
43 #include "coal/BV/OBB.h"
44 #include "coal/BV/RSS.h"
45 #include "coal/BV/OBBRSS.h"
46 #include "coal/BV/kIOS.h"
47 #include "coal/math/transform.h"
48
49 /** @brief Main namespace */
50 namespace coal {
51
52 /// @cond IGNORE
53 namespace details {
54
55 /// @brief Convert a bounding volume of type BV1 in configuration tf1 to a
56 /// bounding volume of type BV2 in I configuration.
57 template <typename BV1, typename BV2>
58 struct Converter {
59 static void convert(const BV1& bv1, const Transform3s& tf1, BV2& bv2);
60 static void convert(const BV1& bv1, BV2& bv2);
61 };
62
63 /// @brief Convert from AABB to AABB, not very tight but is fast.
64 template <>
65 struct Converter<AABB, AABB> {
66 ✗ static void convert(const AABB& bv1, const Transform3s& tf1, AABB& bv2) {
67 ✗ const Vec3s& center = bv1.center();
68 ✗ Scalar r = (bv1.max_ - bv1.min_).norm() * Scalar(0.5);
69 ✗ const Vec3s center2 = tf1.transform(center);
70 ✗ bv2.min_ = center2 - Vec3s::Constant(r);
71 ✗ bv2.max_ = center2 + Vec3s::Constant(r);
72 }
73
74 137330 static void convert(const AABB& bv1, AABB& bv2) { bv2 = bv1; }
75 };
76
77 template <>
78 struct Converter<AABB, OBB> {
79 173179 static void convert(const AABB& bv1, const Transform3s& tf1, OBB& bv2) {
80
1/2
✓ Branch 2 taken 173179 times.
✗ Branch 3 not taken.
 173179 bv2.To = tf1.transform(bv1.center());
81
4/8
✓ Branch 1 taken 173179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 173179 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 173179 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 173179 times.
✗ Branch 11 not taken.
 173179 bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
82 173179 bv2.axes = tf1.getRotation();
83 173179 }
84
85 98318 static void convert(const AABB& bv1, OBB& bv2) {
86 98318 bv2.To = bv1.center();
87
4/8
✓ Branch 1 taken 98318 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 98318 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 98318 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 98318 times.
✗ Branch 11 not taken.
 98318 bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
88 98318 bv2.axes.setIdentity();
89 98318 }
90 };
91
92 template <>
93 struct Converter<OBB, OBB> {
94 28982 static void convert(const OBB& bv1, const Transform3s& tf1, OBB& bv2) {
95 28982 bv2.extent = bv1.extent;
96 28982 bv2.To = tf1.transform(bv1.To);
97
2/4
✓ Branch 3 taken 28982 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 28982 times.
✗ Branch 7 not taken.
 28982 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
98 28982 }
99
100 static void convert(const OBB& bv1, OBB& bv2) { bv2 = bv1; }
101 };
102
103 template <>
104 struct Converter<OBBRSS, OBB> {
105 28982 static void convert(const OBBRSS& bv1, const Transform3s& tf1, OBB& bv2) {
106 28982 Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
107 28982 }
108
109 static void convert(const OBBRSS& bv1, OBB& bv2) {
110 Converter<OBB, OBB>::convert(bv1.obb, bv2);
111 }
112 };
113
114 template <>
115 struct Converter<RSS, OBB> {
116 ✗ static void convert(const RSS& bv1, const Transform3s& tf1, OBB& bv2) {
117 ✗ const Scalar half = Scalar(0.5);
118 ✗ bv2.extent = Vec3s(bv1.length[0] * half + bv1.radius,
119 ✗ bv1.length[1] * half + bv1.radius, bv1.radius);
120 ✗ bv2.To = tf1.transform(bv1.Tr);
121 ✗ bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
122 }
123
124 static void convert(const RSS& bv1, OBB& bv2) {
125 const Scalar half = Scalar(0.5);
126 bv2.extent = Vec3s(bv1.length[0] * half + bv1.radius,
127 bv1.length[1] * half + bv1.radius, bv1.radius);
128 bv2.To = bv1.Tr;
129 bv2.axes = bv1.axes;
130 }
131 };
132
133 template <typename BV1>
134 struct Converter<BV1, AABB> {
135 ✗ static void convert(const BV1& bv1, const Transform3s& tf1, AABB& bv2) {
136 ✗ const Vec3s& center = bv1.center();
137 ✗ const Scalar half = Scalar(0.5);
138 ✗ Scalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * half;
139 ✗ const Vec3s center2 = tf1.transform(center);
140 ✗ bv2.min_ = center2 - Vec3s::Constant(r);
141 ✗ bv2.max_ = center2 + Vec3s::Constant(r);
142 }
143
144 ✗ static void convert(const BV1& bv1, AABB& bv2) {
145 ✗ const Vec3s& center = bv1.center();
146 ✗ const Scalar half = Scalar(0.5);
147 ✗ Scalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * half;
148 ✗ bv2.min_ = center - Vec3s::Constant(r);
149 ✗ bv2.max_ = center + Vec3s::Constant(r);
150 }
151 };
152
153 template <typename BV1>
154 struct Converter<BV1, OBB> {
155 ✗ static void convert(const BV1& bv1, const Transform3s& tf1, OBB& bv2) {
156 ✗ AABB bv;
157 ✗ Converter<BV1, AABB>::convert(bv1, bv);
158 ✗ Converter<AABB, OBB>::convert(bv, tf1, bv2);
159 }
160
161 static void convert(const BV1& bv1, OBB& bv2) {
162 AABB bv;
163 Converter<BV1, AABB>::convert(bv1, bv);
164 Converter<AABB, OBB>::convert(bv, bv2);
165 }
166 };
167
168 template <>
169 struct Converter<OBB, RSS> {
170 static void convert(const OBB& bv1, const Transform3s& tf1, RSS& bv2) {
171 bv2.Tr = tf1.transform(bv1.To);
172 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
173
174 bv2.radius = bv1.extent[2];
175 bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
176 bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
177 }
178
179 static void convert(const OBB& bv1, RSS& bv2) {
180 bv2.Tr = bv1.To;
181 bv2.axes = bv1.axes;
182
183 bv2.radius = bv1.extent[2];
184 bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
185 bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
186 }
187 };
188
189 template <>
190 struct Converter<RSS, RSS> {
191 static void convert(const RSS& bv1, const Transform3s& tf1, RSS& bv2) {
192 bv2.Tr = tf1.transform(bv1.Tr);
193 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
194
195 bv2.radius = bv1.radius;
196 bv2.length[0] = bv1.length[0];
197 bv2.length[1] = bv1.length[1];
198 }
199
200 static void convert(const RSS& bv1, RSS& bv2) { bv2 = bv1; }
201 };
202
203 template <>
204 struct Converter<OBBRSS, RSS> {
205 static void convert(const OBBRSS& bv1, const Transform3s& tf1, RSS& bv2) {
206 Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
207 }
208
209 static void convert(const OBBRSS& bv1, RSS& bv2) {
210 Converter<RSS, RSS>::convert(bv1.rss, bv2);
211 }
212 };
213
214 template <>
215 struct Converter<AABB, RSS> {
216 98318 static void convert(const AABB& bv1, const Transform3s& tf1, RSS& bv2) {
217
2/4
✓ Branch 1 taken 98318 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 98318 times.
✗ Branch 5 not taken.
 98318 bv2.Tr = tf1.transform(bv1.center());
218
219 /// Sort the AABB edges so that AABB extents are ordered.
220
3/6
✓ Branch 1 taken 98318 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 98318 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 98318 times.
✗ Branch 8 not taken.
 98318 Scalar d[3] = {bv1.width(), bv1.height(), bv1.depth()};
221 98318 Eigen::DenseIndex id[3] = {0, 1, 2};
222
223
2/2
✓ Branch 0 taken 196636 times.
✓ Branch 1 taken 98318 times.
 294954 for (Eigen::DenseIndex i = 1; i < 3; ++i) {
224
2/2
✓ Branch 0 taken 294954 times.
✓ Branch 1 taken 196636 times.
 491590 for (Eigen::DenseIndex j = i; j > 0; --j) {
225
2/2
✓ Branch 0 taken 265551 times.
✓ Branch 1 taken 29403 times.
 294954 if (d[j] > d[j - 1]) {
226 {
227 265551 Scalar tmp = d[j];
228 265551 d[j] = d[j - 1];
229 265551 d[j - 1] = tmp;
230 }
231 {
232 265551 Eigen::DenseIndex tmp = id[j];
233 265551 id[j] = id[j - 1];
234 265551 id[j - 1] = tmp;
235 }
236 }
237 }
238 }
239
240
3/6
✓ Branch 1 taken 98318 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 98318 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 98318 times.
✗ Branch 8 not taken.
 98318 const Vec3s extent = (bv1.max_ - bv1.min_) * 0.5;
241
1/2
✓ Branch 1 taken 98318 times.
✗ Branch 2 not taken.
 98318 bv2.radius = extent[id[2]];
242
1/2
✓ Branch 1 taken 98318 times.
✗ Branch 2 not taken.
 98318 bv2.length[0] = (extent[id[0]] - bv2.radius) * 2;
243
1/2
✓ Branch 1 taken 98318 times.
✗ Branch 2 not taken.
 98318 bv2.length[1] = (extent[id[1]] - bv2.radius) * 2;
244
245 98318 const Matrix3s& R = tf1.getRotation();
246 98318 const bool left_hand = (id[0] == (id[1] + 1) % 3);
247
2/2
✓ Branch 0 taken 68941 times.
✓ Branch 1 taken 29377 times.
 98318 if (left_hand)
248
4/8
✓ Branch 1 taken 68941 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 68941 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 68941 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 68941 times.
✗ Branch 11 not taken.
 68941 bv2.axes.col(0) = -R.col(id[0]);
249 else
250
3/6
✓ Branch 1 taken 29377 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29377 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29377 times.
✗ Branch 8 not taken.
 29377 bv2.axes.col(0) = R.col(id[0]);
251
3/6
✓ Branch 1 taken 98318 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 98318 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 98318 times.
✗ Branch 8 not taken.
 98318 bv2.axes.col(1) = R.col(id[1]);
252
3/6
✓ Branch 1 taken 98318 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 98318 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 98318 times.
✗ Branch 8 not taken.
 98318 bv2.axes.col(2) = R.col(id[2]);
253 98318 }
254
255 98318 static void convert(const AABB& bv1, RSS& bv2) {
256
1/2
✓ Branch 2 taken 98318 times.
✗ Branch 3 not taken.
 98318 convert(bv1, Transform3s(), bv2);
257 98318 }
258 };
259
260 template <>
261 struct Converter<AABB, OBBRSS> {
262 static void convert(const AABB& bv1, const Transform3s& tf1, OBBRSS& bv2) {
263 Converter<AABB, OBB>::convert(bv1, tf1, bv2.obb);
264 Converter<AABB, RSS>::convert(bv1, tf1, bv2.rss);
265 }
266
267 98318 static void convert(const AABB& bv1, OBBRSS& bv2) {
268 98318 Converter<AABB, OBB>::convert(bv1, bv2.obb);
269 98318 Converter<AABB, RSS>::convert(bv1, bv2.rss);
270 98318 }
271 };
272
273 } // namespace details
274
275 /// @endcond
276
277 /// @brief Convert a bounding volume of type BV1 in configuration tf1 to
278 /// bounding volume of type BV2 in identity configuration.
279 template <typename BV1, typename BV2>
280 402319 static inline void convertBV(const BV1& bv1, const Transform3s& tf1, BV2& bv2) {
281 402319 details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
282 402319 }
283
284 /// @brief Convert a bounding volume of type BV1 to bounding volume of type BV2
285 /// in identity configuration.
286 template <typename BV1, typename BV2>
287 412294 static inline void convertBV(const BV1& bv1, BV2& bv2) {
288 412294 details::Converter<BV1, BV2>::convert(bv1, bv2);
289 412294 }
290
291 } // namespace coal
292
293 #endif
294