GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Software License Agreement (BSD License) |
||
3 |
* |
||
4 |
* Copyright (c) 2021, INRIA |
||
5 |
* All rights reserved. |
||
6 |
* |
||
7 |
* Redistribution and use in source and binary forms, with or without |
||
8 |
* modification, are permitted provided that the following conditions |
||
9 |
* are met: |
||
10 |
* |
||
11 |
* * Redistributions of source code must retain the above copyright |
||
12 |
* notice, this list of conditions and the following disclaimer. |
||
13 |
* * Redistributions in binary form must reproduce the above |
||
14 |
* copyright notice, this list of conditions and the following |
||
15 |
* disclaimer in the documentation and/or other materials provided |
||
16 |
* with the distribution. |
||
17 |
* * Neither the name of Open Source Robotics Foundation nor the names of its |
||
18 |
* contributors may be used to endorse or promote products derived |
||
19 |
* from this software without specific prior written permission. |
||
20 |
* |
||
21 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||
22 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||
23 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||
24 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||
25 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||
26 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||
27 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
28 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
||
29 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||
30 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||
31 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
32 |
* POSSIBILITY OF SUCH DAMAGE. |
||
33 |
*/ |
||
34 |
|||
35 |
/** \author Justin Carpentier */ |
||
36 |
|||
37 |
#ifndef HPP_FCL_HEIGHT_FIELD_H |
||
38 |
#define HPP_FCL_HEIGHT_FIELD_H |
||
39 |
|||
40 |
#include <hpp/fcl/fwd.hh> |
||
41 |
#include <hpp/fcl/collision_object.h> |
||
42 |
#include <hpp/fcl/BV/BV_node.h> |
||
43 |
#include <hpp/fcl/BVH/BVH_internal.h> |
||
44 |
|||
45 |
#include <vector> |
||
46 |
|||
47 |
namespace hpp { |
||
48 |
namespace fcl { |
||
49 |
|||
50 |
/// @addtogroup Construction_Of_HeightField |
||
51 |
/// @{ |
||
52 |
|||
53 |
struct HPP_FCL_DLLAPI HFNodeBase { |
||
54 |
/// @brief An index for first child node or primitive |
||
55 |
/// If the value is positive, it is the index of the first child bv node |
||
56 |
/// If the value is negative, it is -(primitive index + 1) |
||
57 |
/// Zero is not used. |
||
58 |
size_t first_child; |
||
59 |
|||
60 |
Eigen::DenseIndex x_id, x_size; |
||
61 |
Eigen::DenseIndex y_id, y_size; |
||
62 |
|||
63 |
FCL_REAL max_height; |
||
64 |
|||
65 |
/// @brief Default constructor |
||
66 |
217626 |
HFNodeBase() |
|
67 |
217626 |
: first_child(0), |
|
68 |
x_id(-1), |
||
69 |
x_size(0), |
||
70 |
y_id(-1), |
||
71 |
y_size(0), |
||
72 |
217626 |
max_height(std::numeric_limits<FCL_REAL>::lowest()) {} |
|
73 |
|||
74 |
/// @brief Comparison operator |
||
75 |
354486 |
bool operator==(const HFNodeBase& other) const { |
|
76 |
✓✗ | 354486 |
return first_child == other.first_child && x_id == other.x_id && |
77 |
✓✗✓✗ |
354486 |
x_size == other.x_size && y_id == other.y_id && |
78 |
✓✗✓✗ ✓✗ |
708972 |
y_size == other.y_size && max_height == other.max_height; |
79 |
} |
||
80 |
|||
81 |
/// @brief Difference operator |
||
82 |
bool operator!=(const HFNodeBase& other) const { return !(*this == other); } |
||
83 |
|||
84 |
/// @brief Whether current node is a leaf node (i.e. contains a primitive |
||
85 |
/// index) |
||
86 |
✓✓✓✓ |
136454 |
inline bool isLeaf() const { return x_size == 1 && y_size == 1; } |
87 |
|||
88 |
/// @brief Return the index of the first child. The index is referred to the |
||
89 |
/// bounding volume array (i.e. bvs) in BVHModel |
||
90 |
136971 |
inline size_t leftChild() const { return first_child; } |
|
91 |
|||
92 |
/// @brief Return the index of the second child. The index is referred to the |
||
93 |
/// bounding volume array (i.e. bvs) in BVHModel |
||
94 |
136971 |
inline size_t rightChild() const { return first_child + 1; } |
|
95 |
|||
96 |
inline Eigen::Vector2i leftChildIndexes() const { |
||
97 |
return Eigen::Vector2i(x_id, y_id); |
||
98 |
} |
||
99 |
inline Eigen::Vector2i rightChildIndexes() const { |
||
100 |
return Eigen::Vector2i(x_id + x_size / 2, y_id + y_size / 2); |
||
101 |
} |
||
102 |
}; |
||
103 |
|||
104 |
template <typename BV> |
||
105 |
struct HPP_FCL_DLLAPI HFNode : public HFNodeBase { |
||
106 |
typedef HFNodeBase Base; |
||
107 |
|||
108 |
/// @brief bounding volume storing the geometry |
||
109 |
BV bv; |
||
110 |
|||
111 |
/// @brief Equality operator |
||
112 |
393764 |
bool operator==(const HFNode& other) const { |
|
113 |
✓✗✓✗ |
393764 |
return Base::operator==(other) && bv == other.bv; |
114 |
} |
||
115 |
|||
116 |
/// @brief Difference operator |
||
117 |
bool operator!=(const HFNode& other) const { return !(*this == other); } |
||
118 |
|||
119 |
/// @brief Check whether two BVNode collide |
||
120 |
bool overlap(const HFNode& other) const { return bv.overlap(other.bv); } |
||
121 |
/// @brief Check whether two BVNode collide |
||
122 |
bool overlap(const HFNode& other, const CollisionRequest& request, |
||
123 |
FCL_REAL& sqrDistLowerBound) const { |
||
124 |
return bv.overlap(other.bv, request, sqrDistLowerBound); |
||
125 |
} |
||
126 |
|||
127 |
/// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and |
||
128 |
/// the underlying BV supports distance, return the nearest points. |
||
129 |
FCL_REAL distance(const HFNode& other, Vec3f* P1 = NULL, |
||
130 |
Vec3f* P2 = NULL) const { |
||
131 |
return bv.distance(other.bv, P1, P2); |
||
132 |
} |
||
133 |
|||
134 |
/// @brief Access to the center of the BV |
||
135 |
Vec3f getCenter() const { return bv.center(); } |
||
136 |
|||
137 |
/// @brief Access to the orientation of the BV |
||
138 |
const Matrix3f& getOrientation() const { |
||
139 |
static const Matrix3f id3 = Matrix3f::Identity(); |
||
140 |
return id3; |
||
141 |
} |
||
142 |
|||
143 |
513808 |
virtual ~HFNode() {} |
|
144 |
|||
145 |
/// \cond |
||
146 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
147 |
/// \endcond |
||
148 |
}; |
||
149 |
|||
150 |
namespace details { |
||
151 |
|||
152 |
template <typename BV> |
||
153 |
struct UpdateBoundingVolume { |
||
154 |
98318 |
static void run(const Vec3f& pointA, const Vec3f& pointB, BV& bv) { |
|
155 |
✓✗ | 98318 |
AABB bv_aabb(pointA, pointB); |
156 |
// AABB bv_aabb; |
||
157 |
// bv_aabb.update(pointA,pointB); |
||
158 |
✓✗ | 98318 |
convertBV(bv_aabb, bv); |
159 |
98318 |
} |
|
160 |
}; |
||
161 |
|||
162 |
template <> |
||
163 |
struct UpdateBoundingVolume<AABB> { |
||
164 |
117720 |
static void run(const Vec3f& pointA, const Vec3f& pointB, AABB& bv) { |
|
165 |
✓✗ | 117720 |
AABB bv_aabb(pointA, pointB); |
166 |
✓✗ | 117720 |
convertBV(bv_aabb, bv); |
167 |
// bv.update(pointA,pointB); |
||
168 |
117720 |
} |
|
169 |
}; |
||
170 |
|||
171 |
} // namespace details |
||
172 |
|||
173 |
/// @brief Data structure depicting a height field given by the base grid |
||
174 |
/// dimensions and the elevation along the grid. \tparam BV one of the bounding |
||
175 |
/// volume class in \ref Bounding_Volume. |
||
176 |
/// |
||
177 |
/// An height field is defined by its base dimensions along the X and Y axes and |
||
178 |
/// a set ofpoints defined by their altitude, regularly dispatched on the grid. |
||
179 |
/// The height field is centered at the origin and the corners of the geometry |
||
180 |
/// correspond to the following coordinates [± x_dim/2; ± y_dim/2]. |
||
181 |
template <typename BV> |
||
182 |
class HPP_FCL_DLLAPI HeightField : public CollisionGeometry { |
||
183 |
public: |
||
184 |
typedef CollisionGeometry Base; |
||
185 |
|||
186 |
typedef HFNode<BV> Node; |
||
187 |
typedef std::vector<Node> BVS; |
||
188 |
|||
189 |
/// @brief Constructing an empty HeightField |
||
190 |
2 |
HeightField() |
|
191 |
: CollisionGeometry(), |
||
192 |
2 |
min_height((std::numeric_limits<FCL_REAL>::min)()), |
|
193 |
✓✗✓✗ ✓✗ |
4 |
max_height((std::numeric_limits<FCL_REAL>::max)()) {} |
194 |
|||
195 |
/// @brief Constructing an HeightField from its base dimensions and the set of |
||
196 |
/// heights points. |
||
197 |
/// The granularity of the height field along X and Y direction is |
||
198 |
/// extraded from the Z grid. |
||
199 |
/// |
||
200 |
/// \param[in] x_dim Dimension along the X axis |
||
201 |
/// \param[in] y_dim Dimension along the Y axis |
||
202 |
/// \param[in] heights Matrix containing the altitude of each point compositng |
||
203 |
/// the height field |
||
204 |
/// \param[in] min_height Minimal height of the height field |
||
205 |
/// |
||
206 |
19 |
HeightField(const FCL_REAL x_dim, const FCL_REAL y_dim, |
|
207 |
const MatrixXf& heights, const FCL_REAL min_height = (FCL_REAL)0) |
||
208 |
✓✗✓✗ ✓✗ |
19 |
: CollisionGeometry() { |
209 |
✓✗ | 19 |
init(x_dim, y_dim, heights, min_height); |
210 |
19 |
} |
|
211 |
|||
212 |
/// @brief Copy contructor from another HeightField |
||
213 |
/// |
||
214 |
/// \param[in] other to copy. |
||
215 |
/// |
||
216 |
12 |
HeightField(const HeightField& other) |
|
217 |
: CollisionGeometry(other), |
||
218 |
12 |
x_dim(other.x_dim), |
|
219 |
12 |
y_dim(other.y_dim), |
|
220 |
12 |
heights(other.heights), |
|
221 |
12 |
min_height(other.min_height), |
|
222 |
12 |
max_height(other.max_height), |
|
223 |
12 |
x_grid(other.x_grid), |
|
224 |
12 |
y_grid(other.y_grid), |
|
225 |
12 |
bvs(other.bvs), |
|
226 |
✓✗✓✗ ✓✗✓✗ |
12 |
num_bvs(other.num_bvs) {} |
227 |
|||
228 |
/// @brief Returns a const reference of the grid along the X direction. |
||
229 |
55672 |
const VecXf& getXGrid() const { return x_grid; } |
|
230 |
/// @brief Returns a const reference of the grid along the Y direction. |
||
231 |
55672 |
const VecXf& getYGrid() const { return y_grid; } |
|
232 |
|||
233 |
/// @brief Returns a const reference of the heights |
||
234 |
55656 |
const MatrixXf& getHeights() const { return heights; } |
|
235 |
|||
236 |
/// @brief Returns the dimension of the Height Field along the X direction. |
||
237 |
12 |
FCL_REAL getXDim() const { return x_dim; } |
|
238 |
/// @brief Returns the dimension of the Height Field along the Y direction. |
||
239 |
12 |
FCL_REAL getYDim() const { return y_dim; } |
|
240 |
|||
241 |
/// @brief Returns the minimal height value of the Height Field. |
||
242 |
55656 |
FCL_REAL getMinHeight() const { return min_height; } |
|
243 |
/// @brief Returns the maximal height value of the Height Field. |
||
244 |
FCL_REAL getMaxHeight() const { return max_height; } |
||
245 |
|||
246 |
✓✗ | 12 |
virtual HeightField<BV>* clone() const { return new HeightField(*this); } |
247 |
|||
248 |
/// @brief deconstruction, delete mesh data related. |
||
249 |
48 |
virtual ~HeightField() {} |
|
250 |
|||
251 |
/// @brief Compute the AABB for the HeightField, used for broad-phase |
||
252 |
/// collision |
||
253 |
24 |
void computeLocalAABB() { |
|
254 |
✓✗✓✗ ✓✗ |
24 |
const Vec3f A(x_grid[0], y_grid[0], min_height); |
255 |
✓✗✓✗ ✓✗✓✗ |
24 |
const Vec3f B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1], |
256 |
✓✗ | 24 |
max_height); |
257 |
✓✗ | 24 |
const AABB aabb_(A, B); |
258 |
|||
259 |
✓✗✓✗ |
24 |
aabb_radius = (A - B).norm() / 2.; |
260 |
✓✗ | 24 |
aabb_local = aabb_; |
261 |
✓✗ | 24 |
aabb_center = aabb_.center(); |
262 |
24 |
} |
|
263 |
|||
264 |
/// @brief Update Height Field height |
||
265 |
24 |
void updateHeights(const MatrixXf& new_heights) { |
|
266 |
✓✗✗✓ |
48 |
if (new_heights.rows() != heights.rows() || |
267 |
✗✓ | 24 |
new_heights.cols() != heights.cols()) |
268 |
HPP_FCL_THROW_PRETTY( |
||
269 |
"The matrix containing the new heights values does not have the same " |
||
270 |
"matrix size as the original one.\n" |
||
271 |
"\tinput values - rows: " |
||
272 |
<< new_heights.rows() << " - cols: " << new_heights.cols() << "\n" |
||
273 |
<< "\texpected values - rows: " << heights.rows() |
||
274 |
<< " - cols: " << heights.cols() << "\n", |
||
275 |
std::invalid_argument); |
||
276 |
|||
277 |
✓✗ | 24 |
heights = new_heights.cwiseMax(min_height); |
278 |
24 |
this->max_height = recursiveUpdateHeight(0); |
|
279 |
✗✓ | 24 |
assert(this->max_height == heights.maxCoeff()); |
280 |
24 |
} |
|
281 |
|||
282 |
protected: |
||
283 |
19 |
void init(const FCL_REAL x_dim, const FCL_REAL y_dim, const MatrixXf& heights, |
|
284 |
const FCL_REAL min_height) { |
||
285 |
19 |
this->x_dim = x_dim; |
|
286 |
19 |
this->y_dim = y_dim; |
|
287 |
✓✗ | 19 |
this->heights = heights.cwiseMax(min_height); |
288 |
19 |
this->min_height = min_height; |
|
289 |
19 |
this->max_height = heights.maxCoeff(); |
|
290 |
|||
291 |
19 |
const Eigen::DenseIndex NX = heights.cols(), NY = heights.rows(); |
|
292 |
✗✓ | 19 |
assert(NX >= 2 && "The number of columns is too small."); |
293 |
✗✓ | 19 |
assert(NY >= 2 && "The number of rows is too small."); |
294 |
|||
295 |
✓✗✓✗ |
19 |
x_grid = VecXf::LinSpaced(NX, -0.5 * x_dim, 0.5 * x_dim); |
296 |
✓✗✓✗ |
19 |
y_grid = VecXf::LinSpaced(NY, 0.5 * y_dim, -0.5 * y_dim); |
297 |
|||
298 |
// Allocate BVS |
||
299 |
19 |
const size_t num_tot_bvs = |
|
300 |
19 |
(size_t)(NX * NY) - 1 + (size_t)((NX - 1) * (NY - 1)); |
|
301 |
19 |
bvs.resize(num_tot_bvs); |
|
302 |
19 |
num_bvs = 0; |
|
303 |
|||
304 |
// Build tree |
||
305 |
19 |
buildTree(); |
|
306 |
19 |
} |
|
307 |
|||
308 |
/// @brief Get the object type: it is a HFIELD |
||
309 |
80 |
OBJECT_TYPE getObjectType() const { return OT_HFIELD; } |
|
310 |
|||
311 |
Vec3f computeCOM() const { return Vec3f::Zero(); } |
||
312 |
|||
313 |
FCL_REAL computeVolume() const { return 0; } |
||
314 |
|||
315 |
Matrix3f computeMomentofInertia() const { return Matrix3f::Zero(); } |
||
316 |
|||
317 |
protected: |
||
318 |
/// @brief Dimensions in meters along X and Y directions |
||
319 |
FCL_REAL x_dim, y_dim; |
||
320 |
|||
321 |
/// @brief Elevation values in meters of the Height Field |
||
322 |
MatrixXf heights; |
||
323 |
|||
324 |
/// @brief Minimal height of the Height Field: all values bellow min_height |
||
325 |
/// will be discarded. |
||
326 |
FCL_REAL min_height, max_height; |
||
327 |
|||
328 |
/// @brief Grids along the X and Y directions. Useful for plotting or other |
||
329 |
/// related things. |
||
330 |
VecXf x_grid, y_grid; |
||
331 |
|||
332 |
/// @brief Bounding volume hierarchy |
||
333 |
BVS bvs; |
||
334 |
unsigned int num_bvs; |
||
335 |
|||
336 |
/// @brief Build the bounding volume hierarchy |
||
337 |
19 |
int buildTree() { |
|
338 |
19 |
num_bvs = 1; |
|
339 |
57 |
const FCL_REAL max_recursive_height = |
|
340 |
19 |
recursiveBuildTree(0, 0, heights.cols() - 1, 0, heights.rows() - 1); |
|
341 |
✗✓ | 19 |
assert(max_recursive_height == max_height && |
342 |
"the maximal height is not correct"); |
||
343 |
HPP_FCL_UNUSED_VARIABLE(max_recursive_height); |
||
344 |
|||
345 |
19 |
bvs.resize(num_bvs); |
|
346 |
19 |
return BVH_OK; |
|
347 |
} |
||
348 |
|||
349 |
157112 |
FCL_REAL recursiveUpdateHeight(const size_t bv_id) { |
|
350 |
157112 |
HFNode<BV>& bv_node = bvs[bv_id]; |
|
351 |
|||
352 |
FCL_REAL max_height; |
||
353 |
✓✓ | 157112 |
if (bv_node.isLeaf()) { |
354 |
✓✗✓✗ |
78568 |
max_height = heights.block<2, 2>(bv_node.y_id, bv_node.x_id).maxCoeff(); |
355 |
} else { |
||
356 |
FCL_REAL |
||
357 |
✓✗ | 78544 |
max_left_height = recursiveUpdateHeight(bv_node.leftChild()), |
358 |
✓✗ | 78544 |
max_right_height = recursiveUpdateHeight(bv_node.rightChild()); |
359 |
|||
360 |
78544 |
max_height = (std::max)(max_left_height, max_right_height); |
|
361 |
} |
||
362 |
|||
363 |
157112 |
bv_node.max_height = max_height; |
|
364 |
|||
365 |
✓✗✓✗ ✓✗ |
157112 |
const Vec3f pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height); |
366 |
✓✗ | 157112 |
const Vec3f pointB(x_grid[bv_node.x_id + bv_node.x_size], |
367 |
✓✗✓✗ |
157112 |
y_grid[bv_node.y_id + bv_node.y_size], max_height); |
368 |
|||
369 |
✓✗ | 157112 |
details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv); |
370 |
|||
371 |
157112 |
return max_height; |
|
372 |
} |
||
373 |
|||
374 |
235563 |
FCL_REAL recursiveBuildTree(const size_t bv_id, const Eigen::DenseIndex x_id, |
|
375 |
const Eigen::DenseIndex x_size, |
||
376 |
const Eigen::DenseIndex y_id, |
||
377 |
const Eigen::DenseIndex y_size) { |
||
378 |
✓✗✓✗ |
235563 |
assert(x_id < heights.cols() && "x_id is out of bounds"); |
379 |
✓✗✓✗ |
235563 |
assert(y_id < heights.rows() && "y_id is out of bounds"); |
380 |
✓✗✓✗ |
235563 |
assert(x_size >= 0 && y_size >= 0 && |
381 |
"x_size or y_size are not of correct value"); |
||
382 |
✓✗ | 235563 |
assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension"); |
383 |
|||
384 |
235563 |
HFNode<BV>& bv_node = bvs[bv_id]; |
|
385 |
FCL_REAL max_height; |
||
386 |
✓✓✓✓ |
235563 |
if (x_size == 1 && |
387 |
y_size == 1) // don't build any BV for the current child node |
||
388 |
{ |
||
389 |
✓✗✓✗ |
117791 |
max_height = heights.block<2, 2>(y_id, x_id).maxCoeff(); |
390 |
} else { |
||
391 |
117772 |
bv_node.first_child = num_bvs; |
|
392 |
117772 |
num_bvs += 2; |
|
393 |
|||
394 |
117772 |
FCL_REAL max_left_height = min_height, max_right_height = min_height; |
|
395 |
✓✓ | 117772 |
if (x_size >= y_size) // splitting along the X axis |
396 |
{ |
||
397 |
45122 |
Eigen::DenseIndex x_size_half = x_size / 2; |
|
398 |
✗✓ | 45122 |
if (x_size == 1) x_size_half = 1; |
399 |
✓✗ | 45122 |
max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, |
400 |
x_size_half, y_id, y_size); |
||
401 |
|||
402 |
✓✗ | 45122 |
max_right_height = |
403 |
recursiveBuildTree(bv_node.rightChild(), x_id + x_size_half, |
||
404 |
x_size - x_size_half, y_id, y_size); |
||
405 |
} else // splitting along the Y axis |
||
406 |
{ |
||
407 |
72650 |
Eigen::DenseIndex y_size_half = y_size / 2; |
|
408 |
✗✓ | 72650 |
if (y_size == 1) y_size_half = 1; |
409 |
✓✗ | 72650 |
max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size, |
410 |
y_id, y_size_half); |
||
411 |
|||
412 |
✓✗ | 72650 |
max_right_height = |
413 |
recursiveBuildTree(bv_node.rightChild(), x_id, x_size, |
||
414 |
y_id + y_size_half, y_size - y_size_half); |
||
415 |
} |
||
416 |
|||
417 |
117772 |
max_height = (std::max)(max_left_height, max_right_height); |
|
418 |
} |
||
419 |
|||
420 |
235563 |
bv_node.max_height = max_height; |
|
421 |
// max_height = std::max(max_height,min_height); |
||
422 |
|||
423 |
✓✗✓✗ ✓✗ |
235563 |
const Vec3f pointA(x_grid[x_id], y_grid[y_id], min_height); |
424 |
✓✗✗✓ |
235563 |
assert(x_id + x_size < x_grid.size()); |
425 |
✓✗✗✓ |
235563 |
assert(y_id + y_size < y_grid.size()); |
426 |
✓✗✓✗ ✓✗ |
235563 |
const Vec3f pointB(x_grid[x_id + x_size], y_grid[y_id + y_size], |
427 |
max_height); |
||
428 |
|||
429 |
✓✗ | 235563 |
details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv); |
430 |
235563 |
bv_node.x_id = x_id; |
|
431 |
235563 |
bv_node.y_id = y_id; |
|
432 |
235563 |
bv_node.x_size = x_size; |
|
433 |
235563 |
bv_node.y_size = y_size; |
|
434 |
|||
435 |
235563 |
return max_height; |
|
436 |
} |
||
437 |
|||
438 |
public: |
||
439 |
/// @brief Access the bv giving the its index |
||
440 |
173722 |
const HFNode<BV>& getBV(unsigned int i) const { |
|
441 |
✗✗ | 173722 |
if (i >= num_bvs) |
442 |
HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument); |
||
443 |
173722 |
return bvs[i]; |
|
444 |
} |
||
445 |
|||
446 |
/// @brief Access the bv giving the its index |
||
447 |
HFNode<BV>& getBV(unsigned int i) { |
||
448 |
if (i >= num_bvs) |
||
449 |
HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument); |
||
450 |
return bvs[i]; |
||
451 |
} |
||
452 |
|||
453 |
/// @brief Get the BV type: default is unknown |
||
454 |
NODE_TYPE getNodeType() const { return BV_UNKNOWN; } |
||
455 |
|||
456 |
private: |
||
457 |
20 |
virtual bool isEqual(const CollisionGeometry& _other) const { |
|
458 |
✓✗ | 20 |
const HeightField* other_ptr = dynamic_cast<const HeightField*>(&_other); |
459 |
✗✓ | 20 |
if (other_ptr == nullptr) return false; |
460 |
20 |
const HeightField& other = *other_ptr; |
|
461 |
|||
462 |
✓✗ | 20 |
return x_dim == other.x_dim && y_dim == other.y_dim && |
463 |
✓✗✓✗ |
20 |
heights == other.heights && min_height == other.min_height && |
464 |
✓✗✓✗ |
20 |
max_height == other.max_height && x_grid == other.x_grid && |
465 |
✓✗✓✗ ✓✗ |
60 |
y_grid == other.y_grid && bvs == other.bvs && |
466 |
✓✗ | 40 |
num_bvs == other.num_bvs; |
467 |
} |
||
468 |
|||
469 |
public: |
||
470 |
✗✗ | 24 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
471 |
}; |
||
472 |
|||
473 |
/// @brief Specialization of getNodeType() for HeightField with different BV |
||
474 |
/// types |
||
475 |
template <> |
||
476 |
NODE_TYPE HeightField<AABB>::getNodeType() const; |
||
477 |
|||
478 |
template <> |
||
479 |
NODE_TYPE HeightField<OBB>::getNodeType() const; |
||
480 |
|||
481 |
template <> |
||
482 |
NODE_TYPE HeightField<RSS>::getNodeType() const; |
||
483 |
|||
484 |
template <> |
||
485 |
NODE_TYPE HeightField<kIOS>::getNodeType() const; |
||
486 |
|||
487 |
template <> |
||
488 |
NODE_TYPE HeightField<OBBRSS>::getNodeType() const; |
||
489 |
|||
490 |
template <> |
||
491 |
NODE_TYPE HeightField<KDOP<16> >::getNodeType() const; |
||
492 |
|||
493 |
template <> |
||
494 |
NODE_TYPE HeightField<KDOP<18> >::getNodeType() const; |
||
495 |
|||
496 |
template <> |
||
497 |
NODE_TYPE HeightField<KDOP<24> >::getNodeType() const; |
||
498 |
|||
499 |
/// @} |
||
500 |
|||
501 |
} // namespace fcl |
||
502 |
|||
503 |
} // namespace hpp |
||
504 |
|||
505 |
#endif |
Generated by: GCOVR (Version 4.2) |