Directory: | ./ |
---|---|
File: | include/coal/hfield.h |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 167 | 177 | 94.4% |
Branches: | 117 | 328 | 35.7% |
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/2✓ Branch 2 taken 6 times.
✗ Branch 3 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 |
1/2✓ Branch 2 taken 12 times.
✗ Branch 3 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 |
1/2✓ Branch 2 taken 14 times.
✗ Branch 3 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 |