GCC Code Coverage Report

Directory: ./
File: include/coal/hfield.h
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 167 177 94.4%
Functions: 49 120 40.8%
Branches: 119 338 35.2%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2021-2024, 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 COAL_HEIGHT_FIELD_H
38 #define COAL_HEIGHT_FIELD_H
39
40 #include "coal/fwd.hh"
41 #include "coal/data_types.h"
42 #include "coal/collision_object.h"
43 #include "coal/BV/BV_node.h"
44 #include "coal/BVH/BVH_internal.h"
45
46 #include <vector>
47
48 namespace coal {
49
50 /// @addtogroup Construction_Of_HeightField
51 /// @{
52
53 struct COAL_DLLAPI HFNodeBase {
54 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
55
56 enum class FaceOrientation {
57 TOP = 1,
58 BOTTOM = 1,
59 NORTH = 2,
60 EAST = 4,
61 SOUTH = 8,
62 WEST = 16
63 };
64
65 /// @brief An index for first child node or primitive
66 /// If the value is positive, it is the index of the first child bv node
67 /// If the value is negative, it is -(primitive index + 1)
68 /// Zero is not used.
69 size_t first_child;
70
71 Eigen::DenseIndex x_id, x_size;
72 Eigen::DenseIndex y_id, y_size;
73
74 Scalar max_height;
75 int contact_active_faces;
76
77 /// @brief Default constructor
78 316248 HFNodeBase()
79 316248 : first_child(0),
80 316248 x_id(-1),
81 316248 x_size(0),
82 316248 y_id(-1),
83 316248 y_size(0),
84 316248 max_height(std::numeric_limits<Scalar>::lowest()),
85 316248 contact_active_faces(0) {}
86
87 /// @brief Comparison operator
88 748496 bool operator==(const HFNodeBase& other) const {
89
1/2
✓ Branch 0 taken 748496 times.
✗ Branch 1 not taken.
 748496 return first_child == other.first_child && x_id == other.x_id &&
90
2/4
✓ Branch 0 taken 748496 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 748496 times.
✗ Branch 3 not taken.
 748496 x_size == other.x_size && y_id == other.y_id &&
91
3/6
✓ Branch 0 taken 748496 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 748496 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 748496 times.
✗ Branch 5 not taken.
 2245488 y_size == other.y_size && max_height == other.max_height &&
92
1/2
✓ Branch 0 taken 748496 times.
✗ Branch 1 not taken.
 1496992 contact_active_faces == other.contact_active_faces;
93 }
94
95 /// @brief Difference operator
96 bool operator!=(const HFNodeBase& other) const { return !(*this == other); }
97
98 /// @brief Whether current node is a leaf node (i.e. contains a primitive
99 /// index)
100
4/4
✓ Branch 0 taken 214127 times.
✓ Branch 1 taken 106719 times.
✓ Branch 2 taken 148431 times.
✓ Branch 3 taken 65696 times.
 320846 inline bool isLeaf() const { return x_size == 1 && y_size == 1; }
101
102 /// @brief Return the index of the first child. The index is referred to the
103 /// bounding volume array (i.e. bvs) in BVHModel
104 150132 inline size_t leftChild() const { return first_child; }
105
106 /// @brief Return the index of the second child. The index is referred to the
107 /// bounding volume array (i.e. bvs) in BVHModel
108 149844 inline size_t rightChild() const { return first_child + 1; }
109
110 inline Eigen::Vector2i leftChildIndexes() const {
111 return Eigen::Vector2i(x_id, y_id);
112 }
113 inline Eigen::Vector2i rightChildIndexes() const {
114 return Eigen::Vector2i(x_id + x_size / 2, y_id + y_size / 2);
115 }
116 };
117
118 inline HFNodeBase::FaceOrientation operator&(HFNodeBase::FaceOrientation a,
119 HFNodeBase::FaceOrientation b) {
120 return HFNodeBase::FaceOrientation(int(a) & int(b));
121 }
122
123 183568 inline int operator&(int a, HFNodeBase::FaceOrientation b) {
124 183568 return a & int(b);
125 }
126
127 template <typename BV>
128 struct COAL_DLLAPI HFNode : public HFNodeBase {
129 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
130
131 typedef HFNodeBase Base;
132
133 /// @brief bounding volume storing the geometry
134 BV bv;
135
136 /// @brief Equality operator
137 787774 bool operator==(const HFNode& other) const {
138
2/4
✓ Branch 1 taken 748496 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 748496 times.
✗ Branch 5 not taken.
 787774 return Base::operator==(other) && bv == other.bv;
139 }
140
141 /// @brief Difference operator
142 bool operator!=(const HFNode& other) const { return !(*this == other); }
143
144 /// @brief Check whether two BVNode collide
145 bool overlap(const HFNode& other) const { return bv.overlap(other.bv); }
146 /// @brief Check whether two BVNode collide
147 bool overlap(const HFNode& other, const CollisionRequest& request,
148 Scalar& sqrDistLowerBound) const {
149 return bv.overlap(other.bv, request, sqrDistLowerBound);
150 }
151
152 /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and
153 /// the underlying BV supports distance, return the nearest points.
154 Scalar distance(const HFNode& other, Vec3s* P1 = NULL,
155 Vec3s* P2 = NULL) const {
156 return bv.distance(other.bv, P1, P2);
157 }
158
159 /// @brief Access to the center of the BV
160 Vec3s getCenter() const { return bv.center(); }
161
162 /// @brief Access to the orientation of the BV
163 coal::Matrix3s::IdentityReturnType getOrientation() const {
164 return Matrix3s::Identity();
165 }
166
167 868656 virtual ~HFNode() {}
168 };
169
170 namespace details {
171
172 template <typename BV>
173 struct UpdateBoundingVolume {
174 98318 static void run(const Vec3s& pointA, const Vec3s& pointB, BV& bv) {
175
1/2
✓ Branch 1 taken 98318 times.
✗ Branch 2 not taken.
 98318 AABB bv_aabb(pointA, pointB);
176 // AABB bv_aabb;
177 // bv_aabb.update(pointA,pointB);
178
1/2
✓ Branch 1 taken 98318 times.
✗ Branch 2 not taken.
 98318 convertBV(bv_aabb, bv);
179 98318 }
180 };
181
182 template <>
183 struct UpdateBoundingVolume<AABB> {
184 137330 static void run(const Vec3s& pointA, const Vec3s& pointB, AABB& bv) {
185
1/2
✓ Branch 1 taken 137330 times.
✗ Branch 2 not taken.
 137330 AABB bv_aabb(pointA, pointB);
186
1/2
✓ Branch 1 taken 137330 times.
✗ Branch 2 not taken.
 137330 convertBV(bv_aabb, bv);
187 // bv.update(pointA,pointB);
188 137330 }
189 };
190
191 } // namespace details
192
193 /// @brief Data structure depicting a height field given by the base grid
194 /// dimensions and the elevation along the grid. \tparam BV one of the bounding
195 /// volume class in \ref Bounding_Volume.
196 ///
197 /// An height field is defined by its base dimensions along the X and Y axes and
198 /// a set ofpoints defined by their altitude, regularly dispatched on the grid.
199 /// The height field is centered at the origin and the corners of the geometry
200 /// correspond to the following coordinates [± x_dim/2; ± y_dim/2].
201 template <typename BV>
202 class COAL_DLLAPI HeightField : public CollisionGeometry {
203 public:
204 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
205
206 typedef CollisionGeometry Base;
207
208 typedef HFNode<BV> Node;
209 typedef std::vector<Node, Eigen::aligned_allocator<Node>> BVS;
210
211 /// @brief Constructing an empty HeightField
212 4 HeightField()
213 : CollisionGeometry(),
214 4 min_height((std::numeric_limits<Scalar>::min)()),
215
4/8
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 4 times.
✗ Branch 13 not taken.
 8 max_height((std::numeric_limits<Scalar>::max)()) {}
216
217 /// @brief Constructing an HeightField from its base dimensions and the set of
218 /// heights points.
219 /// The granularity of the height field along X and Y direction is
220 /// extraded from the Z grid.
221 ///
222 /// \param[in] x_dim Dimension along the X axis
223 /// \param[in] y_dim Dimension along the Y axis
224 /// \param[in] heights Matrix containing the altitude of each point compositng
225 /// the height field
226 /// \param[in] min_height Minimal height of the height field
227 ///
228 26 HeightField(const Scalar x_dim, const Scalar y_dim, const MatrixXs& heights,
229 const Scalar min_height = (Scalar)0)
230
4/8
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 14 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 14 times.
✗ Branch 12 not taken.
 26 : CollisionGeometry() {
231
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 26 init(x_dim, y_dim, heights, min_height);
232 26 }
233
234 /// @brief Copy contructor from another HeightField
235 ///
236 /// \param[in] other to copy.
237 ///
238 14 HeightField(const HeightField& other)
239 : CollisionGeometry(other),
240 14 x_dim(other.x_dim),
241 14 y_dim(other.y_dim),
242 14 heights(other.heights),
243 14 min_height(other.min_height),
244 14 max_height(other.max_height),
245
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 14 x_grid(other.x_grid),
246
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 14 y_grid(other.y_grid),
247
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 14 bvs(other.bvs),
248
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 28 num_bvs(other.num_bvs) {}
249
250 /// @brief Returns a const reference of the grid along the X direction.
251 61206 const VecXs& getXGrid() const { return x_grid; }
252 /// @brief Returns a const reference of the grid along the Y direction.
253 61206 const VecXs& getYGrid() const { return y_grid; }
254
255 /// @brief Returns a const reference of the heights
256 32204 const MatrixXs& getHeights() const { return heights; }
257
258 /// @brief Returns the dimension of the Height Field along the X direction.
259 12 Scalar getXDim() const { return x_dim; }
260 /// @brief Returns the dimension of the Height Field along the Y direction.
261 12 Scalar getYDim() const { return y_dim; }
262
263 /// @brief Returns the minimal height value of the Height Field.
264 32204 Scalar getMinHeight() const { return min_height; }
265 /// @brief Returns the maximal height value of the Height Field.
266 ✗ Scalar getMaxHeight() const { return max_height; }
267
268
1/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 12 virtual HeightField<BV>* clone() const { return new HeightField(*this); }
269
270 3 const BVS& getNodes() const { return bvs; }
271
272 /// @brief deconstruction, delete mesh data related.
273 68 virtual ~HeightField() {}
274
275 /// @brief Compute the AABB for the HeightField, used for broad-phase
276 /// collision
277 25 void computeLocalAABB() {
278
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
 25 const Vec3s A(x_grid[0], y_grid[0], min_height);
279
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
 25 const Vec3s B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1],
280 25 max_height);
281
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 25 const AABB aabb_(A, B);
282
283
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 25 aabb_radius = (A - B).norm() / Scalar(2);
284
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 25 aabb_local = aabb_;
285
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 25 aabb_center = aabb_.center();
286 25 }
287
288 /// @brief Update Height Field height
289 24 void updateHeights(const MatrixXs& new_heights) {
290
3/6
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 12 times.
 48 if (new_heights.rows() != heights.rows() ||
291 24 new_heights.cols() != heights.cols())
292 ✗ COAL_THROW_PRETTY(
293 "The matrix containing the new heights values does not have the same "
294 "matrix size as the original one.\n"
295 "\tinput values - rows: "
296 << new_heights.rows() << " - cols: " << new_heights.cols() << "\n"
297 << "\texpected values - rows: " << heights.rows()
298 << " - cols: " << heights.cols() << "\n",
299 std::invalid_argument);
300
301
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 24 heights = new_heights.cwiseMax(min_height);
302 24 this->max_height = recursiveUpdateHeight(0);
303
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
 24 assert(this->max_height == heights.maxCoeff());
304 24 }
305
306 protected:
307 26 void init(const Scalar x_dim, const Scalar y_dim, const MatrixXs& heights,
308 const Scalar min_height) {
309 26 this->x_dim = x_dim;
310 26 this->y_dim = y_dim;
311
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 26 this->heights = heights.cwiseMax(min_height);
312 26 this->min_height = min_height;
313 26 this->max_height = heights.maxCoeff();
314
315 26 const Eigen::DenseIndex NX = heights.cols(), NY = heights.rows();
316
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 14 times.
 26 assert(NX >= 2 && "The number of columns is too small.");
317
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 14 times.
 26 assert(NY >= 2 && "The number of rows is too small.");
318
319 26 const Scalar half = Scalar(0.5);
320
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 26 x_grid = VecXs::LinSpaced(NX, -half * x_dim, half * x_dim);
321
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 26 y_grid = VecXs::LinSpaced(NY, half * y_dim, -half * y_dim);
322
323 // Allocate BVS
324 26 const size_t num_tot_bvs =
325 26 (size_t)(NX * NY) - 1 + (size_t)((NX - 1) * (NY - 1));
326 26 bvs.resize(num_tot_bvs);
327 26 num_bvs = 0;
328
329 // Build tree
330 26 buildTree();
331 26 }
332
333 /// @brief Get the object type: it is a HFIELD
334 2110 OBJECT_TYPE getObjectType() const { return OT_HFIELD; }
335
336 ✗ Vec3s computeCOM() const { return Vec3s::Zero(); }
337
338 ✗ Scalar computeVolume() const { return 0; }
339
340 ✗ Matrix3s computeMomentofInertia() const { return Matrix3s::Zero(); }
341
342 protected:
343 /// @brief Dimensions in meters along X and Y directions
344 Scalar x_dim, y_dim;
345
346 /// @brief Elevation values in meters of the Height Field
347 MatrixXs heights;
348
349 /// @brief Minimal height of the Height Field: all values bellow min_height
350 /// will be discarded.
351 Scalar min_height, max_height;
352
353 /// @brief Grids along the X and Y directions. Useful for plotting or other
354 /// related things.
355 VecXs x_grid, y_grid;
356
357 /// @brief Bounding volume hierarchy
358 BVS bvs;
359 unsigned int num_bvs;
360
361 /// @brief Build the bounding volume hierarchy
362 26 int buildTree() {
363 26 num_bvs = 1;
364 const Scalar max_recursive_height =
365 26 recursiveBuildTree(0, 0, heights.cols() - 1, 0, heights.rows() - 1);
366
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 14 times.
 26 assert(max_recursive_height == max_height &&
367 "the maximal height is not correct");
368 COAL_UNUSED_VARIABLE(max_recursive_height);
369
370 26 bvs.resize(num_bvs);
371 26 return BVH_OK;
372 }
373
374 157112 Scalar recursiveUpdateHeight(const size_t bv_id) {
375 157112 HFNode<BV>& bv_node = bvs[bv_id];
376
377 Scalar max_height;
378
2/2
✓ Branch 1 taken 39284 times.
✓ Branch 2 taken 39272 times.
 157112 if (bv_node.isLeaf()) {
379
2/4
✓ Branch 1 taken 39284 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39284 times.
✗ Branch 5 not taken.
 78568 max_height = heights.block<2, 2>(bv_node.y_id, bv_node.x_id).maxCoeff();
380 } else {
381
1/2
✓ Branch 2 taken 39272 times.
✗ Branch 3 not taken.
 78544 Scalar max_left_height = recursiveUpdateHeight(bv_node.leftChild()),
382
1/2
✓ Branch 2 taken 39272 times.
✗ Branch 3 not taken.
 78544 max_right_height = recursiveUpdateHeight(bv_node.rightChild());
383
384 78544 max_height = (std::max)(max_left_height, max_right_height);
385 }
386
387 157112 bv_node.max_height = max_height;
388
389
3/6
✓ Branch 1 taken 78556 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78556 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 78556 times.
✗ Branch 8 not taken.
 157112 const Vec3s pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height);
390
1/2
✓ Branch 1 taken 78556 times.
✗ Branch 2 not taken.
 157112 const Vec3s pointB(x_grid[bv_node.x_id + bv_node.x_size],
391
2/4
✓ Branch 1 taken 78556 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78556 times.
✗ Branch 5 not taken.
 157112 y_grid[bv_node.y_id + bv_node.y_size], max_height);
392
393
1/2
✓ Branch 1 taken 78556 times.
✗ Branch 2 not taken.
 157112 details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
394
395 157112 return max_height;
396 }
397
398 255182 Scalar recursiveBuildTree(const size_t bv_id, const Eigen::DenseIndex x_id,
399 const Eigen::DenseIndex x_size,
400 const Eigen::DenseIndex y_id,
401 const Eigen::DenseIndex y_size) {
402
1/2
✓ Branch 1 taken 157092 times.
✗ Branch 2 not taken.
 255182 assert(x_id < heights.cols() && "x_id is out of bounds");
403
1/2
✓ Branch 1 taken 157092 times.
✗ Branch 2 not taken.
 255182 assert(y_id < heights.rows() && "y_id is out of bounds");
404
2/4
✓ Branch 0 taken 157092 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 157092 times.
✗ Branch 3 not taken.
 255182 assert(x_size >= 0 && y_size >= 0 &&
405 "x_size or y_size are not of correct value");
406
1/2
✓ Branch 1 taken 157092 times.
✗ Branch 2 not taken.
 255182 assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension");
407
408 255182 HFNode<BV>& bv_node = bvs[bv_id];
409 Scalar max_height;
410
4/4
✓ Branch 0 taken 113073 times.
✓ Branch 1 taken 44019 times.
✓ Branch 2 taken 78553 times.
✓ Branch 3 taken 34520 times.
 255182 if (x_size == 1 &&
411 y_size == 1) // don't build any BV for the current child node
412 {
413
2/4
✓ Branch 1 taken 78553 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78553 times.
✗ Branch 5 not taken.
 127604 max_height = heights.block<2, 2>(y_id, x_id).maxCoeff();
414 } else {
415 127578 bv_node.first_child = num_bvs;
416 127578 num_bvs += 2;
417
418 127578 Scalar max_left_height = min_height, max_right_height = min_height;
419
2/2
✓ Branch 0 taken 30087 times.
✓ Branch 1 taken 48452 times.
 127578 if (x_size >= y_size) // splitting along the X axis
420 {
421 48874 Eigen::DenseIndex x_size_half = x_size / 2;
422
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 30087 times.
 48874 if (x_size == 1) x_size_half = 1;
423
1/2
✓ Branch 2 taken 30087 times.
✗ Branch 3 not taken.
 48874 max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id,
424 x_size_half, y_id, y_size);
425
426 48874 max_right_height =
427
1/2
✓ Branch 2 taken 30087 times.
✗ Branch 3 not taken.
 48874 recursiveBuildTree(bv_node.rightChild(), x_id + x_size_half,
428 x_size - x_size_half, y_id, y_size);
429 } else // splitting along the Y axis
430 {
431 78704 Eigen::DenseIndex y_size_half = y_size / 2;
432
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 48452 times.
 78704 if (y_size == 1) y_size_half = 1;
433
1/2
✓ Branch 2 taken 48452 times.
✗ Branch 3 not taken.
 78704 max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size,
434 y_id, y_size_half);
435
436 78704 max_right_height =
437
1/2
✓ Branch 2 taken 48452 times.
✗ Branch 3 not taken.
 78704 recursiveBuildTree(bv_node.rightChild(), x_id, x_size,
438 y_id + y_size_half, y_size - y_size_half);
439 }
440
441 127578 max_height = (std::max)(max_left_height, max_right_height);
442 }
443
444 255182 bv_node.max_height = max_height;
445 // max_height = std::max(max_height,min_height);
446
447
3/6
✓ Branch 1 taken 157092 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 157092 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 157092 times.
✗ Branch 8 not taken.
 255182 const Vec3s pointA(x_grid[x_id], y_grid[y_id], min_height);
448
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 157092 times.
 255182 assert(x_id + x_size < x_grid.size());
449
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 157092 times.
 255182 assert(y_id + y_size < y_grid.size());
450
3/6
✓ Branch 1 taken 157092 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 157092 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 157092 times.
✗ Branch 8 not taken.
 255182 const Vec3s pointB(x_grid[x_id + x_size], y_grid[y_id + y_size],
451 max_height);
452
453
1/2
✓ Branch 1 taken 157092 times.
✗ Branch 2 not taken.
 255182 details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
454 255182 bv_node.x_id = x_id;
455 255182 bv_node.y_id = y_id;
456 255182 bv_node.x_size = x_size;
457 255182 bv_node.y_size = y_size;
458
459
2/2
✓ Branch 1 taken 78553 times.
✓ Branch 2 taken 78539 times.
 255182 if (bv_node.isLeaf()) {
460 127604 int& contact_active_faces = bv_node.contact_active_faces;
461 127604 contact_active_faces |= int(HFNodeBase::FaceOrientation::TOP);
462 127604 contact_active_faces |= int(HFNodeBase::FaceOrientation::BOTTOM);
463
464
2/2
✓ Branch 0 taken 801 times.
✓ Branch 1 taken 77752 times.
 127604 if (bv_node.x_id == 0) // first col
465 1304 contact_active_faces |= int(HFNodeBase::FaceOrientation::WEST);
466
467
2/2
✓ Branch 0 taken 737 times.
✓ Branch 1 taken 77816 times.
 127604 if (bv_node.y_id == 0) // first row (TOP)
468 1276 contact_active_faces |= int(HFNodeBase::FaceOrientation::NORTH);
469
470
2/2
✓ Branch 1 taken 801 times.
✓ Branch 2 taken 77752 times.
 127604 if (bv_node.x_id + 1 == heights.cols() - 1) // last col
471 1304 contact_active_faces |= int(HFNodeBase::FaceOrientation::EAST);
472
473
2/2
✓ Branch 1 taken 737 times.
✓ Branch 2 taken 77816 times.
 127604 if (bv_node.y_id + 1 == heights.rows() - 1) // last row (BOTTOM)
474 1276 contact_active_faces |= int(HFNodeBase::FaceOrientation::SOUTH);
475 }
476
477 255182 return max_height;
478 }
479
480 public:
481 /// @brief Access the bv giving the its index
482 248000 const HFNode<BV>& getBV(unsigned int i) const {
483
0/2
✗ Branch 0 not taken.
✗ Branch 1 not taken.
 248000 if (i >= num_bvs)
484 ✗ COAL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
485 248000 return bvs[i];
486 }
487
488 /// @brief Access the bv giving the its index
489 ✗ HFNode<BV>& getBV(unsigned int i) {
490 ✗ if (i >= num_bvs)
491 ✗ COAL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
492 ✗ return bvs[i];
493 }
494
495 /// @brief Get the BV type: default is unknown
496 NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
497
498 private:
499 24 virtual bool isEqual(const CollisionGeometry& _other) const {
500
1/2
✓ Branch 0 taken 18 times.
✗ Branch 1 not taken.
 24 const HeightField* other_ptr = dynamic_cast<const HeightField*>(&_other);
501
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 18 times.
 24 if (other_ptr == nullptr) return false;
502 24 const HeightField& other = *other_ptr;
503
504
1/2
✓ Branch 0 taken 18 times.
✗ Branch 1 not taken.
 24 return x_dim == other.x_dim && y_dim == other.y_dim &&
505
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
 24 heights == other.heights && min_height == other.min_height &&
506
2/4
✓ Branch 0 taken 18 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
 24 max_height == other.max_height && x_grid == other.x_grid &&
507
3/6
✓ Branch 0 taken 18 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
 72 y_grid == other.y_grid && bvs == other.bvs &&
508
1/2
✓ Branch 0 taken 18 times.
✗ Branch 1 not taken.
 48 num_bvs == other.num_bvs;
509 }
510 };
511
512 /// @brief Specialization of getNodeType() for HeightField with different BV
513 /// types
514 template <>
515 NODE_TYPE HeightField<AABB>::getNodeType() const;
516
517 template <>
518 NODE_TYPE HeightField<OBB>::getNodeType() const;
519
520 template <>
521 NODE_TYPE HeightField<RSS>::getNodeType() const;
522
523 template <>
524 NODE_TYPE HeightField<kIOS>::getNodeType() const;
525
526 template <>
527 NODE_TYPE HeightField<OBBRSS>::getNodeType() const;
528
529 template <>
530 NODE_TYPE HeightField<KDOP<16>>::getNodeType() const;
531
532 template <>
533 NODE_TYPE HeightField<KDOP<18>>::getNodeType() const;
534
535 template <>
536 NODE_TYPE HeightField<KDOP<24>>::getNodeType() const;
537
538 /// @}
539
540 } // namespace coal
541
542 #endif
543