coal 3.0.1
Coal, The Collision Detection Library. Previously known as HPP-FCL, fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
hfield.h
Go to the documentation of this file.
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
37#ifndef COAL_HEIGHT_FIELD_H
38#define COAL_HEIGHT_FIELD_H
39
40#include "coal/fwd.hh"
41#include "coal/data_types.h"
43#include "coal/BV/BV_node.h"
45
46#include <vector>
47
48namespace coal {
49
52
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
70
71 Eigen::DenseIndex x_id, x_size;
72 Eigen::DenseIndex y_id, y_size;
73
76
79 : first_child(0),
80 x_id(-1),
81 x_size(0),
82 y_id(-1),
83 y_size(0),
84 max_height(std::numeric_limits<Scalar>::lowest()),
85 contact_active_faces(0) {}
86
88 bool operator==(const HFNodeBase& other) const {
89 return first_child == other.first_child && x_id == other.x_id &&
90 x_size == other.x_size && y_id == other.y_id &&
91 y_size == other.y_size && max_height == other.max_height &&
92 contact_active_faces == other.contact_active_faces;
93 }
94
96 bool operator!=(const HFNodeBase& other) const { return !(*this == other); }
97
100 inline bool isLeaf() const { return x_size == 1 && y_size == 1; }
101
104 inline size_t leftChild() const { return first_child; }
105
108 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
122
124 return a & int(b);
125}
126
127template <typename BV>
129 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
130
132
134 BV bv;
135
137 bool operator==(const HFNode& other) const {
138 return Base::operator==(other) && bv == other.bv;
139 }
140
142 bool operator!=(const HFNode& other) const { return !(*this == other); }
143
145 bool overlap(const HFNode& other) const { return bv.overlap(other.bv); }
147 bool overlap(const HFNode& other, const CollisionRequest& request,
148 Scalar& sqrDistLowerBound) const {
149 return bv.overlap(other.bv, request, sqrDistLowerBound);
150 }
151
154 Scalar distance(const HFNode& other, Vec3s* P1 = NULL,
155 Vec3s* P2 = NULL) const {
156 return bv.distance(other.bv, P1, P2);
157 }
158
160 Vec3s getCenter() const { return bv.center(); }
161
163 coal::Matrix3s::IdentityReturnType getOrientation() const {
164 return Matrix3s::Identity();
165 }
166
167 virtual ~HFNode() {}
168};
169
170namespace details {
171
172template <typename BV>
174 static void run(const Vec3s& pointA, const Vec3s& pointB, BV& bv) {
175 AABB bv_aabb(pointA, pointB);
176 // AABB bv_aabb;
177 // bv_aabb.update(pointA,pointB);
178 convertBV(bv_aabb, bv);
179 }
180};
181
182template <>
184 static void run(const Vec3s& pointA, const Vec3s& pointB, AABB& bv) {
185 AABB bv_aabb(pointA, pointB);
186 convertBV(bv_aabb, bv);
187 // bv.update(pointA,pointB);
188 }
189};
190
191} // namespace details
192
201template <typename BV>
203 public:
204 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
205
207
209 typedef std::vector<Node, Eigen::aligned_allocator<Node>> BVS;
210
214 min_height((std::numeric_limits<Scalar>::min)()),
215 max_height((std::numeric_limits<Scalar>::max)()) {}
216
228 HeightField(const Scalar x_dim, const Scalar y_dim, const MatrixXs& heights,
229 const Scalar min_height = (Scalar)0)
231 init(x_dim, y_dim, heights, min_height);
232 }
233
239 : CollisionGeometry(other),
240 x_dim(other.x_dim),
241 y_dim(other.y_dim),
242 heights(other.heights),
243 min_height(other.min_height),
244 max_height(other.max_height),
245 x_grid(other.x_grid),
246 y_grid(other.y_grid),
247 bvs(other.bvs),
248 num_bvs(other.num_bvs) {}
249
251 const VecXs& getXGrid() const { return x_grid; }
253 const VecXs& getYGrid() const { return y_grid; }
254
256 const MatrixXs& getHeights() const { return heights; }
257
259 Scalar getXDim() const { return x_dim; }
261 Scalar getYDim() const { return y_dim; }
262
264 Scalar getMinHeight() const { return min_height; }
266 Scalar getMaxHeight() const { return max_height; }
267
268 virtual HeightField<BV>* clone() const { return new HeightField(*this); }
269
270 const BVS& getNodes() const { return bvs; }
271
273 virtual ~HeightField() {}
274
278 const Vec3s A(x_grid[0], y_grid[0], min_height);
279 const Vec3s B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1],
280 max_height);
281 const AABB aabb_(A, B);
282
283 aabb_radius = (A - B).norm() / Scalar(2);
284 aabb_local = aabb_;
285 aabb_center = aabb_.center();
286 }
287
289 void updateHeights(const MatrixXs& new_heights) {
290 if (new_heights.rows() != heights.rows() ||
291 new_heights.cols() != heights.cols())
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 heights = new_heights.cwiseMax(min_height);
302 this->max_height = recursiveUpdateHeight(0);
303 assert(this->max_height == heights.maxCoeff());
304 }
305
306 protected:
307 void init(const Scalar x_dim, const Scalar y_dim, const MatrixXs& heights,
308 const Scalar min_height) {
309 this->x_dim = x_dim;
310 this->y_dim = y_dim;
311 this->heights = heights.cwiseMax(min_height);
312 this->min_height = min_height;
313 this->max_height = heights.maxCoeff();
314
315 const Eigen::DenseIndex NX = heights.cols(), NY = heights.rows();
316 assert(NX >= 2 && "The number of columns is too small.");
317 assert(NY >= 2 && "The number of rows is too small.");
318
319 const Scalar half = Scalar(0.5);
320 x_grid = VecXs::LinSpaced(NX, -half * x_dim, half * x_dim);
321 y_grid = VecXs::LinSpaced(NY, half * y_dim, -half * y_dim);
322
323 // Allocate BVS
324 const size_t num_tot_bvs =
325 (size_t)(NX * NY) - 1 + (size_t)((NX - 1) * (NY - 1));
326 bvs.resize(num_tot_bvs);
327 num_bvs = 0;
328
329 // Build tree
330 buildTree();
331 }
332
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:
344 Scalar x_dim, y_dim;
345
348
351 Scalar min_height, max_height;
352
355 VecXs x_grid, y_grid;
356
359 unsigned int num_bvs;
360
362 int buildTree() {
363 num_bvs = 1;
364 const Scalar max_recursive_height =
365 recursiveBuildTree(0, 0, heights.cols() - 1, 0, heights.rows() - 1);
366 assert(max_recursive_height == max_height &&
367 "the maximal height is not correct");
368 COAL_UNUSED_VARIABLE(max_recursive_height);
369
370 bvs.resize(num_bvs);
371 return BVH_OK;
372 }
373
374 Scalar recursiveUpdateHeight(const size_t bv_id) {
375 HFNode<BV>& bv_node = bvs[bv_id];
376
377 Scalar max_height;
378 if (bv_node.isLeaf()) {
379 max_height = heights.block<2, 2>(bv_node.y_id, bv_node.x_id).maxCoeff();
380 } else {
381 Scalar max_left_height = recursiveUpdateHeight(bv_node.leftChild()),
382 max_right_height = recursiveUpdateHeight(bv_node.rightChild());
383
384 max_height = (std::max)(max_left_height, max_right_height);
385 }
386
387 bv_node.max_height = max_height;
388
389 const Vec3s pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height);
390 const Vec3s pointB(x_grid[bv_node.x_id + bv_node.x_size],
391 y_grid[bv_node.y_id + bv_node.y_size], max_height);
392
393 details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
394
395 return max_height;
396 }
397
398 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 assert(x_id < heights.cols() && "x_id is out of bounds");
403 assert(y_id < heights.rows() && "y_id is out of bounds");
404 assert(x_size >= 0 && y_size >= 0 &&
405 "x_size or y_size are not of correct value");
406 assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension");
407
408 HFNode<BV>& bv_node = bvs[bv_id];
409 Scalar max_height;
410 if (x_size == 1 &&
411 y_size == 1) // don't build any BV for the current child node
412 {
413 max_height = heights.block<2, 2>(y_id, x_id).maxCoeff();
414 } else {
415 bv_node.first_child = num_bvs;
416 num_bvs += 2;
417
418 Scalar max_left_height = min_height, max_right_height = min_height;
419 if (x_size >= y_size) // splitting along the X axis
420 {
421 Eigen::DenseIndex x_size_half = x_size / 2;
422 if (x_size == 1) x_size_half = 1;
423 max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id,
424 x_size_half, y_id, y_size);
425
426 max_right_height =
427 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 Eigen::DenseIndex y_size_half = y_size / 2;
432 if (y_size == 1) y_size_half = 1;
433 max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size,
434 y_id, y_size_half);
435
436 max_right_height =
437 recursiveBuildTree(bv_node.rightChild(), x_id, x_size,
438 y_id + y_size_half, y_size - y_size_half);
439 }
440
441 max_height = (std::max)(max_left_height, max_right_height);
442 }
443
444 bv_node.max_height = max_height;
445 // max_height = std::max(max_height,min_height);
446
447 const Vec3s pointA(x_grid[x_id], y_grid[y_id], min_height);
448 assert(x_id + x_size < x_grid.size());
449 assert(y_id + y_size < y_grid.size());
450 const Vec3s pointB(x_grid[x_id + x_size], y_grid[y_id + y_size],
451 max_height);
452
453 details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
454 bv_node.x_id = x_id;
455 bv_node.y_id = y_id;
456 bv_node.x_size = x_size;
457 bv_node.y_size = y_size;
458
459 if (bv_node.isLeaf()) {
460 int& contact_active_faces = bv_node.contact_active_faces;
461 contact_active_faces |= int(HFNodeBase::FaceOrientation::TOP);
462 contact_active_faces |= int(HFNodeBase::FaceOrientation::BOTTOM);
463
464 if (bv_node.x_id == 0) // first col
465 contact_active_faces |= int(HFNodeBase::FaceOrientation::WEST);
466
467 if (bv_node.y_id == 0) // first row (TOP)
468 contact_active_faces |= int(HFNodeBase::FaceOrientation::NORTH);
469
470 if (bv_node.x_id + 1 == heights.cols() - 1) // last col
471 contact_active_faces |= int(HFNodeBase::FaceOrientation::EAST);
472
473 if (bv_node.y_id + 1 == heights.rows() - 1) // last row (BOTTOM)
474 contact_active_faces |= int(HFNodeBase::FaceOrientation::SOUTH);
475 }
476
477 return max_height;
478 }
479
480 public:
482 const HFNode<BV>& getBV(unsigned int i) const {
483 if (i >= num_bvs)
484 COAL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
485 return bvs[i];
486 }
487
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
496 NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
497
498 private:
499 virtual bool isEqual(const CollisionGeometry& _other) const {
500 const HeightField* other_ptr = dynamic_cast<const HeightField*>(&_other);
501 if (other_ptr == nullptr) return false;
502 const HeightField& other = *other_ptr;
503
504 return x_dim == other.x_dim && y_dim == other.y_dim &&
505 heights == other.heights && min_height == other.min_height &&
506 max_height == other.max_height && x_grid == other.x_grid &&
507 y_grid == other.y_grid && bvs == other.bvs &&
508 num_bvs == other.num_bvs;
509 }
510};
511
514template <>
516
517template <>
519
520template <>
522
523template <>
525
526template <>
528
529template <>
530NODE_TYPE HeightField<KDOP<16>>::getNodeType() const;
531
532template <>
533NODE_TYPE HeightField<KDOP<18>>::getNodeType() const;
534
535template <>
536NODE_TYPE HeightField<KDOP<24>>::getNodeType() const;
537
539
540} // namespace coal
541
542#endif
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition AABB.h:55
Vec3s center() const
Center of the AABB.
Definition AABB.h:164
The geometry for the object for collision or distance computation.
Definition collision_object.h:96
Data structure depicting a height field given by the base grid dimensions and the elevation along the...
Definition hfield.h:202
HeightField(const HeightField &other)
Copy contructor from another HeightField.
Definition hfield.h:238
OBJECT_TYPE getObjectType() const
Get the object type: it is a HFIELD.
Definition hfield.h:334
VecXs y_grid
Definition hfield.h:355
const BVS & getNodes() const
Definition hfield.h:270
Scalar max_height
Definition hfield.h:351
const VecXs & getXGrid() const
Returns a const reference of the grid along the X direction.
Definition hfield.h:251
virtual ~HeightField()
deconstruction, delete mesh data related.
Definition hfield.h:273
Scalar getYDim() const
Returns the dimension of the Height Field along the Y direction.
Definition hfield.h:261
std::vector< Node, Eigen::aligned_allocator< Node > > BVS
Definition hfield.h:209
HFNode< BV > Node
Definition hfield.h:208
VecXs x_grid
Grids along the X and Y directions. Useful for plotting or other related things.
Definition hfield.h:355
Scalar computeVolume() const
compute the volume
Definition hfield.h:338
HFNode< BV > & getBV(unsigned int i)
Access the bv giving the its index.
Definition hfield.h:489
const MatrixXs & getHeights() const
Returns a const reference of the heights.
Definition hfield.h:256
void init(const Scalar x_dim, const Scalar y_dim, const MatrixXs &heights, const Scalar min_height)
Definition hfield.h:307
HeightField()
Constructing an empty HeightField.
Definition hfield.h:212
unsigned int num_bvs
Definition hfield.h:359
MatrixXs heights
Elevation values in meters of the Height Field.
Definition hfield.h:347
int buildTree()
Build the bounding volume hierarchy.
Definition hfield.h:362
Scalar getMaxHeight() const
Returns the maximal height value of the Height Field.
Definition hfield.h:266
Scalar y_dim
Definition hfield.h:344
const VecXs & getYGrid() const
Returns a const reference of the grid along the Y direction.
Definition hfield.h:253
Vec3s computeCOM() const
compute center of mass
Definition hfield.h:336
NODE_TYPE getNodeType() const
Get the BV type: default is unknown.
Definition hfield.h:496
virtual HeightField< BV > * clone() const
Clone *this into a new CollisionGeometry.
Definition hfield.h:268
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef CollisionGeometry Base
Definition hfield.h:206
Scalar min_height
Minimal height of the Height Field: all values bellow min_height will be discarded.
Definition hfield.h:351
Scalar recursiveBuildTree(const size_t bv_id, const Eigen::DenseIndex x_id, const Eigen::DenseIndex x_size, const Eigen::DenseIndex y_id, const Eigen::DenseIndex y_size)
Definition hfield.h:398
void computeLocalAABB()
Compute the AABB for the HeightField, used for broad-phase collision.
Definition hfield.h:277
Scalar x_dim
Dimensions in meters along X and Y directions.
Definition hfield.h:344
Matrix3s computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition hfield.h:340
const HFNode< BV > & getBV(unsigned int i) const
Access the bv giving the its index.
Definition hfield.h:482
Scalar recursiveUpdateHeight(const size_t bv_id)
Definition hfield.h:374
Scalar getXDim() const
Returns the dimension of the Height Field along the X direction.
Definition hfield.h:259
HeightField(const Scalar x_dim, const Scalar y_dim, const MatrixXs &heights, const Scalar min_height=(Scalar) 0)
Constructing an HeightField from its base dimensions and the set of heights points....
Definition hfield.h:228
void updateHeights(const MatrixXs &new_heights)
Update Height Field height.
Definition hfield.h:289
BVS bvs
Bounding volume hierarchy.
Definition hfield.h:358
Scalar getMinHeight() const
Returns the minimal height value of the Height Field.
Definition hfield.h:264
#define COAL_DLLAPI
Definition config.hh:88
#define COAL_UNUSED_VARIABLE(var)
Definition fwd.hh:56
#define COAL_THROW_PRETTY(message, exception)
Definition fwd.hh:64
@ BV_UNKNOWN
Definition collision_object.h:65
@ OT_HFIELD
Definition collision_object.h:57
Main namespace.
Definition broadphase_bruteforce.h:44
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VecXs
Definition data_types.h:73
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition collision_object.h:64
CollisionRequestFlag operator&(CollisionRequestFlag a, CollisionRequestFlag b)
Definition collision_data.h:1204
Eigen::Matrix< Scalar, 3, 1 > Vec3s
Definition data_types.h:70
double Scalar
Definition data_types.h:68
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > MatrixXs
Definition data_types.h:79
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition collision_object.h:52
@ BVH_OK
Definition BVH_internal.h:64
Eigen::Matrix< Scalar, 3, 3 > Matrix3s
Definition data_types.h:74
bool isEqual(const Eigen::MatrixBase< Derived > &lhs, const Eigen::MatrixBase< OtherDerived > &rhs, const Scalar tol=std::numeric_limits< Scalar >::epsilon() *100)
Definition tools.h:204
request to the collision algorithm
Definition collision_data.h:311
Definition hfield.h:53
Eigen::DenseIndex y_size
Definition hfield.h:72
size_t rightChild() const
Return the index of the second child. The index is referred to the bounding volume array (i....
Definition hfield.h:108
Eigen::DenseIndex x_size
Definition hfield.h:71
int contact_active_faces
Definition hfield.h:75
Eigen::Vector2i rightChildIndexes() const
Definition hfield.h:113
bool isLeaf() const
Whether current node is a leaf node (i.e. contains a primitive index)
Definition hfield.h:100
Scalar max_height
Definition hfield.h:74
bool operator==(const HFNodeBase &other) const
Comparison operator.
Definition hfield.h:88
size_t first_child
An index for first child node or primitive If the value is positive, it is the index of the first chi...
Definition hfield.h:69
HFNodeBase()
Default constructor.
Definition hfield.h:78
Eigen::DenseIndex y_id
Definition hfield.h:72
bool operator!=(const HFNodeBase &other) const
Difference operator.
Definition hfield.h:96
Eigen::DenseIndex x_id
Definition hfield.h:71
FaceOrientation
Definition hfield.h:56
size_t leftChild() const
Return the index of the first child. The index is referred to the bounding volume array (i....
Definition hfield.h:104
Eigen::Vector2i leftChildIndexes() const
Definition hfield.h:110
Definition hfield.h:128
bool overlap(const HFNode &other) const
Check whether two BVNode collide.
Definition hfield.h:145
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef HFNodeBase Base
Definition hfield.h:131
bool overlap(const HFNode &other, const CollisionRequest &request, Scalar &sqrDistLowerBound) const
Check whether two BVNode collide.
Definition hfield.h:147
bool operator!=(const HFNode &other) const
Difference operator.
Definition hfield.h:142
bool operator==(const HFNode &other) const
Equality operator.
Definition hfield.h:137
virtual ~HFNode()
Definition hfield.h:167
coal::Matrix3s::IdentityReturnType getOrientation() const
Access to the orientation of the BV.
Definition hfield.h:163
Vec3s getCenter() const
Access to the center of the BV.
Definition hfield.h:160
Scalar distance(const HFNode &other, Vec3s *P1=NULL, Vec3s *P2=NULL) const
Compute the distance between two BVNode. P1 and P2, if not NULL and the underlying BV supports distan...
Definition hfield.h:154
BV bv
bounding volume storing the geometry
Definition hfield.h:134
static void run(const Vec3s &pointA, const Vec3s &pointB, AABB &bv)
Definition hfield.h:184
static void run(const Vec3s &pointA, const Vec3s &pointB, BV &bv)
Definition hfield.h:174