GCC Code Coverage Report


Directory: ./
File: src/BV/kDOP.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 64 110 58.2%
Branches: 35 152 23.0%

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 #include "coal/collision_data.h"
39 #include "coal/BV/kDOP.h"
40
41 #include <limits>
42 #include <iostream>
43
44 namespace coal {
45
46 /// @brief Find the smaller and larger one of two values
47 inline void minmax(Scalar a, Scalar b, Scalar& minv, Scalar& maxv) {
48 if (a > b) {
49 minv = b;
50 maxv = a;
51 } else {
52 minv = a;
53 maxv = b;
54 }
55 }
56 /// @brief Merge the interval [minv, maxv] and value p/
57 90674648 inline void minmax(Scalar p, Scalar& minv, Scalar& maxv) {
58
2/2
✓ Branch 0 taken 12011964 times.
✓ Branch 1 taken 78662684 times.
90674648 if (p > maxv) maxv = p;
59
2/2
✓ Branch 0 taken 11591225 times.
✓ Branch 1 taken 79083423 times.
90674648 if (p < minv) minv = p;
60 90674648 }
61
62 /// @brief Compute the distances to planes with normals from KDOP vectors except
63 /// those of AABB face planes
64 template <short N>
65 void getDistances(const Vec3s& /*p*/, Scalar* /*d*/) {}
66
67 /// @brief Specification of getDistances
68 template <>
69 3126712 inline void getDistances<5>(const Vec3s& p, Scalar* d) {
70 3126712 d[0] = p[0] + p[1];
71 3126712 d[1] = p[0] + p[2];
72 3126712 d[2] = p[1] + p[2];
73 3126712 d[3] = p[0] - p[1];
74 3126712 d[4] = p[0] - p[2];
75 3126712 }
76
77 template <>
78 3126712 inline void getDistances<6>(const Vec3s& p, Scalar* d) {
79 3126712 d[0] = p[0] + p[1];
80 3126712 d[1] = p[0] + p[2];
81 3126712 d[2] = p[1] + p[2];
82 3126712 d[3] = p[0] - p[1];
83 3126712 d[4] = p[0] - p[2];
84 3126712 d[5] = p[1] - p[2];
85 3126712 }
86
87 template <>
88 3126712 inline void getDistances<9>(const Vec3s& p, Scalar* d) {
89 3126712 d[0] = p[0] + p[1];
90 3126712 d[1] = p[0] + p[2];
91 3126712 d[2] = p[1] + p[2];
92 3126712 d[3] = p[0] - p[1];
93 3126712 d[4] = p[0] - p[2];
94 3126712 d[5] = p[1] - p[2];
95 3126712 d[6] = p[0] + p[1] - p[2];
96 3126712 d[7] = p[0] + p[2] - p[1];
97 3126712 d[8] = p[1] + p[2] - p[0];
98 3126712 }
99
100 template <short N>
101 1681932 KDOP<N>::KDOP() {
102 1681932 Scalar real_max = (std::numeric_limits<Scalar>::max)();
103
2/4
✓ Branch 1 taken 840966 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 840966 times.
✗ Branch 5 not taken.
1681932 dist_.template head<N / 2>().setConstant(real_max);
104
2/4
✓ Branch 1 taken 840966 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 840966 times.
✗ Branch 5 not taken.
1681932 dist_.template tail<N / 2>().setConstant(-real_max);
105 1681932 }
106
107 template <short N>
108 KDOP<N>::KDOP(const Vec3s& v) {
109 for (short i = 0; i < 3; ++i) {
110 dist_[i] = dist_[N / 2 + i] = v[i];
111 }
112
113 Scalar d[(N - 6) / 2];
114 getDistances<(N - 6) / 2>(v, d);
115 for (short i = 0; i < (N - 6) / 2; ++i) {
116 dist_[3 + i] = dist_[3 + i + N / 2] = d[i];
117 }
118 }
119
120 template <short N>
121 KDOP<N>::KDOP(const Vec3s& a, const Vec3s& b) {
122 for (short i = 0; i < 3; ++i) {
123 minmax(a[i], b[i], dist_[i], dist_[i + N / 2]);
124 }
125
126 Scalar ad[(N - 6) / 2], bd[(N - 6) / 2];
127 getDistances<(N - 6) / 2>(a, ad);
128 getDistances<(N - 6) / 2>(b, bd);
129 for (short i = 0; i < (N - 6) / 2; ++i) {
130 minmax(ad[i], bd[i], dist_[3 + i], dist_[3 + i + N / 2]);
131 }
132 }
133
134 template <short N>
135 bool KDOP<N>::overlap(const KDOP<N>& other) const {
136 if ((dist_.template head<N / 2>() > other.dist_.template tail<N / 2>()).any())
137 return false;
138 if ((dist_.template tail<N / 2>() < other.dist_.template head<N / 2>()).any())
139 return false;
140 return true;
141 }
142
143 template <short N>
144 41454528 bool KDOP<N>::overlap(const KDOP<N>& other, const CollisionRequest& request,
145 Scalar& sqrDistLowerBound) const {
146 41454528 const Scalar breakDistance(request.break_distance + request.security_margin);
147
148
2/4
✓ Branch 1 taken 20727264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20727264 times.
✗ Branch 5 not taken.
41454528 Scalar a = (dist_.template head<N / 2>() - other.dist_.template tail<N / 2>())
149
2/4
✓ Branch 1 taken 20727264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20727264 times.
✗ Branch 5 not taken.
41454528 .minCoeff();
150
2/2
✓ Branch 0 taken 8744 times.
✓ Branch 1 taken 20718520 times.
41454528 if (a > breakDistance) {
151 17488 sqrDistLowerBound = a * a;
152 17488 return false;
153 }
154
155
2/4
✓ Branch 1 taken 20718520 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20718520 times.
✗ Branch 5 not taken.
41437040 Scalar b = (other.dist_.template head<N / 2>() - dist_.template tail<N / 2>())
156
2/4
✓ Branch 1 taken 20718520 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20718520 times.
✗ Branch 5 not taken.
41437040 .minCoeff();
157
2/2
✓ Branch 0 taken 16115 times.
✓ Branch 1 taken 20702405 times.
41437040 if (b > breakDistance) {
158 32230 sqrDistLowerBound = b * b;
159 32230 return false;
160 }
161
162 41404810 sqrDistLowerBound = std::min(a, b);
163 41404810 return true;
164 }
165
166 template <short N>
167 bool KDOP<N>::inside(const Vec3s& p) const {
168 if ((p.array() < dist_.template head<3>()).any()) return false;
169 if ((p.array() > dist_.template segment<3>(N / 2)).any()) return false;
170
171 enum { P = ((N - 6) / 2) };
172 Eigen::Array<Scalar, P, 1> d;
173 getDistances<P>(p, d.data());
174
175 if ((d < dist_.template segment<P>(3)).any()) return false;
176 if ((d > dist_.template segment<P>(3 + N / 2)).any()) return false;
177
178 return true;
179 }
180
181 template <short N>
182 18760272 KDOP<N>& KDOP<N>::operator+=(const Vec3s& p) {
183
2/2
✓ Branch 0 taken 28140408 times.
✓ Branch 1 taken 9380136 times.
75041088 for (short i = 0; i < 3; ++i) {
184
3/6
✓ Branch 1 taken 28140408 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28140408 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28140408 times.
✗ Branch 8 not taken.
56280816 minmax(p[i], dist_[i], dist_[N / 2 + i]);
185 }
186
187 Scalar pd[(N - 6) / 2];
188
1/2
✓ Branch 1 taken 9380136 times.
✗ Branch 2 not taken.
18760272 getDistances<(N - 6) / 2>(p, pd);
189
2/2
✓ Branch 0 taken 62534240 times.
✓ Branch 1 taken 9380136 times.
143828752 for (short i = 0; i < (N - 6) / 2; ++i) {
190
2/4
✓ Branch 1 taken 62534240 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62534240 times.
✗ Branch 5 not taken.
125068480 minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]);
191 }
192
193 18760272 return *this;
194 }
195
196 template <short N>
197 39222 KDOP<N>& KDOP<N>::operator+=(const KDOP<N>& other) {
198
2/2
✓ Branch 0 taken 189573 times.
✓ Branch 1 taken 19611 times.
418368 for (short i = 0; i < N / 2; ++i) {
199 379146 dist_[i] = std::min(other.dist_[i], dist_[i]);
200 379146 dist_[i + N / 2] = std::max(other.dist_[i + N / 2], dist_[i + N / 2]);
201 }
202 39222 return *this;
203 }
204
205 template <short N>
206 39222 KDOP<N> KDOP<N>::operator+(const KDOP<N>& other) const {
207
1/2
✓ Branch 1 taken 19611 times.
✗ Branch 2 not taken.
39222 KDOP<N> res(*this);
208
2/4
✓ Branch 1 taken 19611 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19611 times.
✗ Branch 5 not taken.
78444 return res += other;
209 }
210
211 template <short N>
212 Scalar KDOP<N>::distance(const KDOP<N>& /*other*/, Vec3s* /*P*/,
213 Vec3s* /*Q*/) const {
214 std::cerr << "KDOP distance not implemented!" << std::endl;
215 return 0.0;
216 }
217
218 template <short N>
219 KDOP<N> translate(const KDOP<N>& bv, const Vec3s& t) {
220 KDOP<N> res(bv);
221 for (short i = 0; i < 3; ++i) {
222 res.dist(i) += t[i];
223 res.dist(short(N / 2 + i)) += t[i];
224 }
225
226 Scalar d[(N - 6) / 2];
227 getDistances<(N - 6) / 2>(t, d);
228 for (short i = 0; i < (N - 6) / 2; ++i) {
229 res.dist(short(3 + i)) += d[i];
230 res.dist(short(3 + i + N / 2)) += d[i];
231 }
232
233 return res;
234 }
235
236 template class KDOP<16>;
237 template class KDOP<18>;
238 template class KDOP<24>;
239
240 template KDOP<16> translate<16>(const KDOP<16>&, const Vec3s&);
241 template KDOP<18> translate<18>(const KDOP<18>&, const Vec3s&);
242 template KDOP<24> translate<24>(const KDOP<24>&, const Vec3s&);
243
244 } // namespace coal
245