GCC Code Coverage Report


Directory: ./
File: include/coal/BV/kDOP.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 7 14 50.0%
Branches: 5 14 35.7%

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_KDOP_H
39 #define COAL_KDOP_H
40
41 #include "coal/fwd.hh"
42 #include "coal/data_types.h"
43
44 namespace coal {
45
46 struct CollisionRequest;
47
48 /// @addtogroup Bounding_Volume
49 /// @{
50
51 /// @brief KDOP class describes the KDOP collision structures. K is set as the
52 /// template parameter, which should be 16, 18, or 24
53 /// The KDOP structure is defined by some pairs of parallel planes defined by
54 /// some axes.
55 /// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off
56 /// some space of the edges:
57 /// (-1,0,0) and (1,0,0) -> indices 0 and 8
58 /// (0,-1,0) and (0,1,0) -> indices 1 and 9
59 /// (0,0,-1) and (0,0,1) -> indices 2 and 10
60 /// (-1,-1,0) and (1,1,0) -> indices 3 and 11
61 /// (-1,0,-1) and (1,0,1) -> indices 4 and 12
62 /// (0,-1,-1) and (0,1,1) -> indices 5 and 13
63 /// (-1,1,0) and (1,-1,0) -> indices 6 and 14
64 /// (-1,0,1) and (1,0,-1) -> indices 7 and 15
65 /// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off
66 /// some space of the edges:
67 /// (-1,0,0) and (1,0,0) -> indices 0 and 9
68 /// (0,-1,0) and (0,1,0) -> indices 1 and 10
69 /// (0,0,-1) and (0,0,1) -> indices 2 and 11
70 /// (-1,-1,0) and (1,1,0) -> indices 3 and 12
71 /// (-1,0,-1) and (1,0,1) -> indices 4 and 13
72 /// (0,-1,-1) and (0,1,1) -> indices 5 and 14
73 /// (-1,1,0) and (1,-1,0) -> indices 6 and 15
74 /// (-1,0,1) and (1,0,-1) -> indices 7 and 16
75 /// (0,-1,1) and (0,1,-1) -> indices 8 and 17
76 /// For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off
77 /// some space of the edges:
78 /// (-1,0,0) and (1,0,0) -> indices 0 and 12
79 /// (0,-1,0) and (0,1,0) -> indices 1 and 13
80 /// (0,0,-1) and (0,0,1) -> indices 2 and 14
81 /// (-1,-1,0) and (1,1,0) -> indices 3 and 15
82 /// (-1,0,-1) and (1,0,1) -> indices 4 and 16
83 /// (0,-1,-1) and (0,1,1) -> indices 5 and 17
84 /// (-1,1,0) and (1,-1,0) -> indices 6 and 18
85 /// (-1,0,1) and (1,0,-1) -> indices 7 and 19
86 /// (0,-1,1) and (0,1,-1) -> indices 8 and 20
87 /// (-1, -1, 1) and (1, 1, -1) --> indices 9 and 21
88 /// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22
89 /// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23
90 template <short N>
91 class COAL_DLLAPI KDOP {
92 protected:
93 /// @brief Origin's distances to N KDOP planes
94 Eigen::Array<Scalar, N, 1> dist_;
95
96 public:
97 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
98
99 /// @brief Creating kDOP containing nothing
100 KDOP();
101
102 /// @brief Creating kDOP containing only one point
103 KDOP(const Vec3s& v);
104
105 /// @brief Creating kDOP containing two points
106 KDOP(const Vec3s& a, const Vec3s& b);
107
108 /// @brief Equality operator
109 bool operator==(const KDOP& other) const {
110 return (dist_ == other.dist_).all();
111 }
112
113 /// @brief Difference operator
114 bool operator!=(const KDOP& other) const {
115 return (dist_ != other.dist_).any();
116 }
117
118 /// @brief Check whether two KDOPs overlap.
119 bool overlap(const KDOP<N>& other) const;
120
121 /// @brief Check whether two KDOPs overlap.
122 /// @return true if collision happens.
123 /// @retval sqrDistLowerBound squared lower bound on distance between boxes if
124 /// they do not overlap.
125 bool overlap(const KDOP<N>& other, const CollisionRequest& request,
126 Scalar& sqrDistLowerBound) const;
127
128 /// @brief The distance between two KDOP<N>. Not implemented.
129 Scalar distance(const KDOP<N>& other, Vec3s* P = NULL, Vec3s* Q = NULL) const;
130
131 /// @brief Merge the point and the KDOP
132 KDOP<N>& operator+=(const Vec3s& p);
133
134 /// @brief Merge two KDOPs
135 KDOP<N>& operator+=(const KDOP<N>& other);
136
137 /// @brief Create a KDOP by mergin two KDOPs
138 KDOP<N> operator+(const KDOP<N>& other) const;
139
140 /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs)
141 82809620 inline Scalar size() const {
142 82809620 return width() * width() + height() * height() + depth() * depth();
143 }
144
145 /// @brief The (AABB) center
146 253488 inline Vec3s center() const {
147
5/10
✓ Branch 1 taken 126744 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126744 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126744 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 126744 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 126744 times.
✗ Branch 14 not taken.
253488 return (dist_.template head<3>() + dist_.template segment<3>(N / 2)) / 2;
148 }
149
150 /// @brief The (AABB) width
151 167461054 inline Scalar width() const { return dist_[N / 2] - dist_[0]; }
152
153 /// @brief The (AABB) height
154 167437564 inline Scalar height() const { return dist_[N / 2 + 1] - dist_[1]; }
155
156 /// @brief The (AABB) depth
157 166441546 inline Scalar depth() const { return dist_[N / 2 + 2] - dist_[2]; }
158
159 /// @brief The (AABB) volume
160 inline Scalar volume() const { return width() * height() * depth(); }
161
162 inline Scalar dist(short i) const { return dist_[i]; }
163
164 inline Scalar& dist(short i) { return dist_[i]; }
165
166 //// @brief Check whether one point is inside the KDOP
167 bool inside(const Vec3s& p) const;
168 };
169
170 template <short N>
171 bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP<N>& /*b1*/,
172 const KDOP<N>& /*b2*/) {
173 COAL_THROW_PRETTY("not implemented", std::logic_error);
174 }
175
176 template <short N>
177 bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP<N>& /*b1*/,
178 const KDOP<N>& /*b2*/, const CollisionRequest& /*request*/,
179 Scalar& /*sqrDistLowerBound*/) {
180 COAL_THROW_PRETTY("not implemented", std::logic_error);
181 }
182
183 /// @brief translate the KDOP BV
184 template <short N>
185 COAL_DLLAPI KDOP<N> translate(const KDOP<N>& bv, const Vec3s& t);
186
187 } // namespace coal
188
189 #endif
190