hpp-fcl 1.8.1
HPP 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, 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 HPP_FCL_HEIGHT_FIELD_H
38#define HPP_FCL_HEIGHT_FIELD_H
39
40#include <hpp/fcl/fwd.hh>
42#include <hpp/fcl/BV/BV_node.h>
44
45#include <boost/integer/integer_log2.hpp>
46#include <vector>
47
48namespace hpp
49{
50namespace fcl
51{
52
55
57{
63
64 Eigen::DenseIndex x_id, x_size;
65 Eigen::DenseIndex y_id, y_size;
66
69 : first_child(0)
70 , x_id(-1), x_size(0)
71 , y_id(-1), y_size(0)
72 {}
73
75 bool operator==(const HFNodeBase & other) const
76 {
77 return
78 first_child == other.first_child
79 && x_id == other.x_id
80 && x_size == other.x_size
81 && y_id == other.y_id
82 && y_size == other.y_size;
83 }
84
86 bool operator!=(const HFNodeBase & other) const
87 {
88 return !(*this == other);
89 }
90
92 inline bool isLeaf() const { return x_size == 1 && y_size == 1; }
93
95 inline size_t leftChild() const { return first_child; }
96
98 inline size_t rightChild() const { return first_child + 1; }
99
100 inline Eigen::Vector2i leftChildIndexes() const { return Eigen::Vector2i(x_id,y_id); }
101 inline Eigen::Vector2i rightChildIndexes() const { return Eigen::Vector2i(x_id + x_size/2, y_id + y_size/2); }
102};
103
104template<typename BV>
106: public HFNodeBase
107{
109
111 BV bv;
112
114 bool operator==(const HFNode & other) const
115 {
116 return Base::operator==(other) && bv == other.bv;
117 }
118
120 bool operator!=(const HFNode & other) const
121 {
122 return !(*this == other);
123 }
124
126 bool overlap(const HFNode& other) const
127 {
128 return bv.overlap(other.bv);
129 }
131 bool overlap(const HFNode& other, const CollisionRequest& request,
132 FCL_REAL& sqrDistLowerBound) const
133 {
134 return bv.overlap(other.bv, request, sqrDistLowerBound);
135 }
136
138 FCL_REAL distance(const HFNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
139 {
140 return bv.distance(other.bv, P1, P2);
141 }
142
144 Vec3f getCenter() const { return bv.center(); }
145
148 {
149 static const Matrix3f id3 = Matrix3f::Identity();
150 return id3;
151 }
152
153 virtual ~HFNode() {}
154
156 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
158};
159
160namespace details
161{
162
163 template<typename BV>
165 {
166 static void run(const Vec3f & pointA, const Vec3f & pointB, BV & bv)
167 {
168 AABB bv_aabb(pointA,pointB);
169// AABB bv_aabb;
170// bv_aabb.update(pointA,pointB);
171 convertBV(bv_aabb,bv);
172 }
173 };
174
175 template<>
177 {
178 static void run(const Vec3f & pointA, const Vec3f & pointB, AABB & bv)
179 {
180 AABB bv_aabb(pointA,pointB);
181 convertBV(bv_aabb,bv);
182// bv.update(pointA,pointB);
183 }
184 };
185
186}
187
193template<typename BV>
195: public CollisionGeometry
196{
197public:
198
200
202 typedef std::vector<Node> BVS;
203
207 , min_height((std::numeric_limits<FCL_REAL>::min)())
208 , max_height((std::numeric_limits<FCL_REAL>::max)())
209 {}
210
220 const FCL_REAL y_dim,
221 const MatrixXf & heights,
222 const FCL_REAL min_height = (FCL_REAL)0)
224 {
225 init(x_dim,y_dim,heights,min_height);
226 }
227
233 : CollisionGeometry(other)
234 , x_dim(other.x_dim)
235 , y_dim(other.y_dim)
236 , heights(other.heights)
237 , min_height(other.min_height)
238 , max_height(other.max_height)
239 , x_grid(other.x_grid)
240 , y_grid(other.y_grid)
241 , bvs(other.bvs)
242 , num_bvs(other.num_bvs)
243 {
244
245 }
246
248 const VecXf & getXGrid() const { return x_grid; }
250 const VecXf & getYGrid() const { return y_grid; }
251
253 const MatrixXf & getHeights() const { return heights; }
254
256 FCL_REAL getXDim() const { return x_dim; }
258 FCL_REAL getYDim() const { return y_dim; }
259
261 FCL_REAL getMinHeight() const { return min_height; }
263 FCL_REAL getMaxHeight() const { return max_height; }
264
265 virtual HeightField<BV> * clone() const
266 {
267 return new HeightField(*this);
268 }
269
271 virtual ~HeightField() {}
272
275 {
276 Vec3f A(x_grid[0],y_grid[0],min_height);
277 Vec3f B(x_grid[x_grid.size()-1],y_grid[y_grid.size()-1],max_height);
278 AABB aabb_(A,B);
279
280 aabb_radius = (A-B).norm()/2.;
281 aabb_local = aabb_;
282 }
283
285 bool operator==(const HeightField & other) const
286 {
287 bool res = Base::operator==(other);
288 if(!res)
289 return false;
290
291 res |=
292 heights == other.heights
293 && x_dim == other.x_dim
294 && y_dim == other.y_dim
295 && min_height == other.min_height
296 && max_height == other.max_height
297 && x_grid == other.x_grid
298 && y_grid == other.y_grid
299 && bvs == other.bvs
300 && num_bvs == other.num_bvs
301 ;
302
303 return res;
304 }
305
307 bool operator!=(const HeightField & other) const
308 {
309 return !(*this == other);
310 }
311
313 void updateHeights(const MatrixXf & new_heights)
314 {
315 if(new_heights.rows() != heights.rows() || new_heights.cols() != heights.cols())
316 HPP_FCL_THROW_PRETTY("The matrix containing the new heights values does not have the same matrix size as the original one.\n"
317 "\tinput values - rows: " << new_heights.rows() << " - cols: " << new_heights.cols() << "\n" <<
318 "\texpected values - rows: " << heights.rows() << " - cols: " << heights.cols() << "\n"
319 ,std::invalid_argument);
320
321 heights = new_heights.cwiseMax(min_height);
322 this->max_height = recursiveUpdateHeight(0);
323 assert(this->max_height == heights.maxCoeff());
324 }
325
326protected:
327
328 void init(const FCL_REAL x_dim,
329 const FCL_REAL y_dim,
330 const MatrixXf & heights,
331 const FCL_REAL min_height)
332 {
333 this->x_dim = x_dim;
334 this->y_dim = y_dim;
335 this->heights = heights.cwiseMax(min_height);
336 this->min_height = min_height;
337 this->max_height = heights.maxCoeff();
338
339 const Eigen::DenseIndex
340 NX = heights.cols(),
341 NY = heights.rows();
342 assert(NX >= 2 && "The number of columns is too small.");
343 assert(NY >= 2 && "The number of rows is too small.");
344
345 x_grid = VecXf::LinSpaced(NX,-0.5*x_dim,0.5*x_dim);
346 y_grid = VecXf::LinSpaced(NY,0.5*y_dim,-0.5*y_dim);
347
348 // Allocate BVS
349 const size_t num_tot_bvs = (size_t)(NX * NY) - 1 + (size_t)((NX-1) * (NY-1));
350 bvs.resize(num_tot_bvs);
351 num_bvs = 0;
352
353 // Build tree
354 buildTree();
355 }
356
359
361 {
362 return Vec3f::Zero();
363 }
364
366 {
367 return 0;
368 }
369
371 {
372 return Matrix3f::Zero();
373 }
374
375protected:
376
379
382
384 FCL_REAL min_height, max_height;
385
387 VecXf x_grid, y_grid;
388
391 unsigned int num_bvs;
392
395 {
396 num_bvs = 1;
397 const FCL_REAL max_recursive_height = recursiveBuildTree(0, 0, heights.cols()-1, 0, heights.rows()-1);
398 assert(max_recursive_height == max_height && "the maximal height is not correct");
399 HPP_FCL_UNUSED_VARIABLE(max_recursive_height);
400
401 bvs.resize(num_bvs);
402 return BVH_OK;
403 }
404
405 FCL_REAL recursiveUpdateHeight(const size_t bv_id)
406 {
407 HFNode<BV> & bv_node = bvs[bv_id];
408
409 FCL_REAL max_height;
410 if(bv_node.isLeaf())
411 {
412 max_height = heights.block<2,2>(bv_node.y_id,bv_node.x_id).maxCoeff();
413 }
414 else
415 {
417 max_left_height = recursiveUpdateHeight(bv_node.leftChild()),
418 max_right_height = recursiveUpdateHeight(bv_node.rightChild());
419
420 max_height = (std::max)(max_left_height,max_right_height);
421 }
422
423 const Vec3f pointA(x_grid[bv_node.x_id],y_grid[bv_node.y_id],min_height);
424 const Vec3f pointB(x_grid[bv_node.x_id+bv_node.x_size],y_grid[bv_node.y_id+bv_node.y_size],max_height);
425
426 details::UpdateBoundingVolume<BV>::run(pointA,pointB,bv_node.bv);
427
428 return max_height;
429 }
430
431 FCL_REAL recursiveBuildTree(const size_t bv_id,
432 const Eigen::DenseIndex x_id, const Eigen::DenseIndex x_size,
433 const Eigen::DenseIndex y_id, const Eigen::DenseIndex y_size)
434 {
435 assert(x_id < heights.cols() && "x_id is out of bounds");
436 assert(y_id < heights.rows() && "y_id is out of bounds");
437 assert(x_size >= 0 && y_size >= 0 && "x_size or y_size are not of correct value");
438 assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension");
439
440 HFNode<BV> & bvnode = bvs[bv_id];
441 FCL_REAL max_height;
442 if(x_size == 1 && y_size == 1) // don't build any BV for the current child node
443 {
444 max_height = heights.block<2,2>(y_id,x_id).maxCoeff();
445 }
446 else
447 {
448 bvnode.first_child = num_bvs;
449 num_bvs += 2;
450
451 FCL_REAL max_left_height = min_height, max_right_height = min_height;
452 if(x_size >= y_size) // splitting along the X axis
453 {
454 Eigen::DenseIndex x_size_half = x_size/2;
455 if(x_size == 1) x_size_half = 1;
456 max_left_height = recursiveBuildTree(bvnode.leftChild(),
457 x_id, x_size_half,
458 y_id, y_size);
459
460 max_right_height = recursiveBuildTree(bvnode.rightChild(),
461 x_id + x_size_half, x_size - x_size_half,
462 y_id, y_size);
463 }
464 else // splitting along the Y axis
465 {
466 Eigen::DenseIndex y_size_half = y_size/2;
467 if(y_size == 1) y_size_half = 1;
468 max_left_height = recursiveBuildTree(bvnode.leftChild(),
469 x_id, x_size,
470 y_id, y_size_half);
471
472 max_right_height = recursiveBuildTree(bvnode.rightChild(),
473 x_id, x_size,
474 y_id + y_size_half, y_size - y_size_half);
475 }
476
477 max_height = (std::max)(max_left_height,max_right_height);
478 }
479
480// max_height = std::max(max_height,min_height);
481
482 const Vec3f pointA(x_grid[x_id],y_grid[y_id],min_height);
483 assert(x_id+x_size < x_grid.size());
484 assert(y_id+y_size < y_grid.size());
485 const Vec3f pointB(x_grid[x_id+x_size],y_grid[y_id+y_size],max_height);
486
487 details::UpdateBoundingVolume<BV>::run(pointA,pointB,bvnode.bv);
488 bvnode.x_id = x_id; bvnode.y_id = y_id;
489 bvnode.x_size = x_size; bvnode.y_size = y_size;
490
491 return max_height;
492 }
493
494public:
496 const HFNode<BV> & getBV(unsigned int i) const
497 {
498 if(i >= num_bvs)
499 HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
500 return bvs[i];
501 }
502
504 HFNode<BV> & getBV(unsigned int i)
505 {
506 if(i >= num_bvs)
507 HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
508 return bvs[i];
509 }
510
512 NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
513
514};
515
517template<>
519
520template<>
522
523template<>
525
526template<>
528
529template<>
531
532template<>
533NODE_TYPE HeightField<KDOP<16> >::getNodeType() const;
534
535template<>
536NODE_TYPE HeightField<KDOP<18> >::getNodeType() const;
537
538template<>
539NODE_TYPE HeightField<KDOP<24> >::getNodeType() const;
540
542
543}
544
545} // namespace hpp
546
547#endif
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:56
The geometry for the object for collision or distance computation.
Definition: collision_object.h:67
Data structure depicting a height field given by the base grid dimensions and the elevation along the...
Definition: hfield.h:196
FCL_REAL max_height
Definition: hfield.h:384
FCL_REAL 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:431
FCL_REAL y_dim
Definition: hfield.h:378
void init(const FCL_REAL x_dim, const FCL_REAL y_dim, const MatrixXf &heights, const FCL_REAL min_height)
Definition: hfield.h:328
FCL_REAL computeVolume() const
compute the volume
Definition: hfield.h:365
FCL_REAL min_height
Minimal height of the Height Field: all values bellow min_height will be discarded.
Definition: hfield.h:384
FCL_REAL x_dim
Dimensions in meters along X and Y directions.
Definition: hfield.h:378
const HFNode< BV > & getBV(unsigned int i) const
Access the bv giving the its index.
Definition: hfield.h:496
VecXf y_grid
Definition: hfield.h:387
virtual HeightField< BV > * clone() const
Clone *this into a new CollisionGeometry.
Definition: hfield.h:265
Vec3f computeCOM() const
compute center of mass
Definition: hfield.h:360
FCL_REAL getXDim() const
Returns the dimension of the Height Field along the X direction.
Definition: hfield.h:256
const VecXf & getYGrid() const
Returns a const reference of the grid along the Y direction.
Definition: hfield.h:250
OBJECT_TYPE getObjectType() const
Get the object type: it is a HFIELD.
Definition: hfield.h:358
HFNode< BV > & getBV(unsigned int i)
Access the bv giving the its index.
Definition: hfield.h:504
FCL_REAL getYDim() const
Returns the dimension of the Height Field along the Y direction.
Definition: hfield.h:258
int buildTree()
Build the bounding volume hierarchy.
Definition: hfield.h:394
FCL_REAL getMinHeight() const
Returns the minimal height value of the Height Field.
Definition: hfield.h:261
bool operator!=(const HeightField &other) const
Difference operator.
Definition: hfield.h:307
HeightField(const HeightField &other)
Copy contructor from another HeightField.
Definition: hfield.h:232
HFNode< BV > Node
Definition: hfield.h:201
CollisionGeometry Base
Definition: hfield.h:199
BVS bvs
Bounding volume hierarchy.
Definition: hfield.h:390
FCL_REAL recursiveUpdateHeight(const size_t bv_id)
Definition: hfield.h:405
const VecXf & getXGrid() const
Returns a const reference of the grid along the X direction.
Definition: hfield.h:248
VecXf x_grid
Grids along the X and Y directions. Useful for plotting or other related things.
Definition: hfield.h:387
void computeLocalAABB()
Compute the AABB for the HeightField, used for broad-phase collision.
Definition: hfield.h:274
virtual ~HeightField()
deconstruction, delete mesh data related.
Definition: hfield.h:271
const MatrixXf & getHeights() const
Returns a const reference of the heights.
Definition: hfield.h:253
std::vector< Node > BVS
Definition: hfield.h:202
NODE_TYPE getNodeType() const
Get the BV type: default is unknown.
Definition: hfield.h:512
HeightField(const FCL_REAL x_dim, const FCL_REAL y_dim, const MatrixXf &heights, const FCL_REAL min_height=(FCL_REAL) 0)
Constructing an HeightField from its base dimensions and the set of heights points....
Definition: hfield.h:219
unsigned int num_bvs
Definition: hfield.h:391
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: hfield.h:370
HeightField()
Constructing an empty HeightField.
Definition: hfield.h:205
bool operator==(const HeightField &other) const
Comparison operators.
Definition: hfield.h:285
MatrixXf heights
Elevation values in meters of the Height Field.
Definition: hfield.h:381
void updateHeights(const MatrixXf &new_heights)
Update Height Field height.
Definition: hfield.h:313
FCL_REAL getMaxHeight() const
Returns the maximal height value of the Height Field.
Definition: hfield.h:263
#define HPP_FCL_DLLAPI
Definition: config.hh:64
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: fwd.hh:53
#define HPP_FCL_UNUSED_VARIABLE(var)
Definition: fwd.hh:51
@ OT_HFIELD
Definition: collision_object.h:55
@ BV_UNKNOWN
Definition: collision_object.h:58
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, Eigen::Dynamic > MatrixXf
Definition: data_types.h:72
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:55
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, 1 > VecXf
Definition: data_types.h:68
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:58
double FCL_REAL
Definition: data_types.h:66
@ BVH_OK
Definition: BVH_internal.h:65
Main namespace.
Definition: AABB.h:44
request to the collision algorithm
Definition: collision_data.h:210
Definition: hfield.h:57
size_t leftChild() const
Return the index of the first child. The index is referred to the bounding volume array (i....
Definition: hfield.h:95
size_t rightChild() const
Return the index of the second child. The index is referred to the bounding volume array (i....
Definition: hfield.h:98
Eigen::Vector2i leftChildIndexes() const
Definition: hfield.h:100
Eigen::DenseIndex y_size
Definition: hfield.h:65
bool operator!=(const HFNodeBase &other) const
Difference operator.
Definition: hfield.h:86
Eigen::DenseIndex x_id
Definition: hfield.h:64
bool isLeaf() const
Whether current node is a leaf node (i.e. contains a primitive index)
Definition: hfield.h:92
Eigen::DenseIndex x_size
Definition: hfield.h:64
Eigen::DenseIndex y_id
Definition: hfield.h:65
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:62
bool operator==(const HFNodeBase &other) const
Comparison operator.
Definition: hfield.h:75
HFNodeBase()
Default constructor.
Definition: hfield.h:68
Eigen::Vector2i rightChildIndexes() const
Definition: hfield.h:101
Definition: hfield.h:107
FCL_REAL distance(const HFNode &other, Vec3f *P1=NULL, Vec3f *P2=NULL) const
Compute the distance between two BVNode. P1 and P2, if not NULL and the underlying BV supports distan...
Definition: hfield.h:138
bool overlap(const HFNode &other) const
Check whether two BVNode collide.
Definition: hfield.h:126
Vec3f getCenter() const
Access to the center of the BV.
Definition: hfield.h:144
bool overlap(const HFNode &other, const CollisionRequest &request, FCL_REAL &sqrDistLowerBound) const
Check whether two BVNode collide.
Definition: hfield.h:131
bool operator!=(const HFNode &other) const
Difference operator.
Definition: hfield.h:120
HFNodeBase Base
Definition: hfield.h:108
const Matrix3f & getOrientation() const
Access to the orientation of the BV.
Definition: hfield.h:147
bool operator==(const HFNode &other) const
Equality operator.
Definition: hfield.h:114
virtual ~HFNode()
Definition: hfield.h:153
BV bv
bounding volume storing the geometry
Definition: hfield.h:111
static void run(const Vec3f &pointA, const Vec3f &pointB, AABB &bv)
Definition: hfield.h:178
static void run(const Vec3f &pointA, const Vec3f &pointB, BV &bv)
Definition: hfield.h:166