hpp-fcl  1.8.1
HPP fork of FCL -- The Flexible Collision Library
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 
48 namespace hpp
49 {
50 namespace fcl
51 {
52 
55 
57 {
62  size_t first_child;
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 
104 template<typename BV>
106 : public HFNodeBase
107 {
108  typedef HFNodeBase Base;
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 
147  const Matrix3f& getOrientation() const
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 
160 namespace 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 
193 template<typename BV>
195 : public CollisionGeometry
196 {
197 public:
198 
200 
201  typedef HFNode<BV> Node;
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 
219  HeightField(const FCL_REAL x_dim,
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 
232  HeightField(const HeightField & other)
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 
326 protected:
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 
358  OBJECT_TYPE getObjectType() const { return OT_HFIELD; }
359 
361  {
362  return Vec3f::Zero();
363  }
364 
366  {
367  return 0;
368  }
369 
371  {
372  return Matrix3f::Zero();
373  }
374 
375 protected:
376 
378  FCL_REAL x_dim, y_dim;
379 
382 
384  FCL_REAL min_height, max_height;
385 
387  VecXf x_grid, y_grid;
388 
390  BVS bvs;
391  unsigned int num_bvs;
392 
394  int buildTree()
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  {
416  FCL_REAL
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 
494 public:
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 
517 template<>
519 
520 template<>
522 
523 template<>
525 
526 template<>
528 
529 template<>
531 
532 template<>
533 NODE_TYPE HeightField<KDOP<16> >::getNodeType() const;
534 
535 template<>
536 NODE_TYPE HeightField<KDOP<18> >::getNodeType() const;
537 
538 template<>
539 NODE_TYPE HeightField<KDOP<24> >::getNodeType() const;
540 
542 
543 }
544 
545 } // namespace hpp
546 
547 #endif
bool overlap(const HFNode &other) const
Check whether two BVNode collide.
Definition: hfield.h:126
FCL_REAL getYDim() const
Returns the dimension of the Height Field along the Y direction.
Definition: hfield.h:258
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, Eigen::Dynamic > MatrixXf
Definition: data_types.h:72
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: hfield.h:370
bool operator!=(const HFNodeBase &other) const
Difference operator.
Definition: hfield.h:86
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. The granularity of the height field along X and Y direction is extraded from the Z grid.
Definition: hfield.h:219
#define HPP_FCL_UNUSED_VARIABLE(var)
Definition: fwd.hh:51
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:55
OBJECT_TYPE getObjectType() const
Get the object type: it is a HFIELD.
Definition: hfield.h:358
Main namespace.
Definition: AABB.h:43
virtual HeightField< BV > * clone() const
Clone *this into a new CollisionGeometry.
Definition: hfield.h:265
FCL_REAL min_height
Minimal height of the Height Field: all values bellow min_height will be discarded.
Definition: hfield.h:384
Definition: collision_object.h:58
bool operator==(const HFNode &other) const
Equality operator.
Definition: hfield.h:114
bool operator!=(const HeightField &other) const
Difference operator.
Definition: hfield.h:307
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, 1 > VecXf
Definition: data_types.h:68
bool operator!=(const HFNode &other) const
Difference operator.
Definition: hfield.h:120
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
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
FCL_REAL getXDim() const
Returns the dimension of the Height Field along the X direction.
Definition: hfield.h:256
FCL_REAL computeVolume() const
compute the volume
Definition: hfield.h:365
bool operator==(const HFNodeBase &other) const
Comparison operator.
Definition: hfield.h:75
bool operator==(const HeightField &other) const
Comparison operators.
Definition: hfield.h:285
Definition: hfield.h:56
void computeLocalAABB()
Compute the AABB for the HeightField, used for broad-phase collision.
Definition: hfield.h:274
FCL_REAL y_dim
Definition: hfield.h:378
BV bv
bounding volume storing the geometry
Definition: hfield.h:111
Definition: hfield.h:105
virtual ~HFNode()
Definition: hfield.h:153
const VecXf & getXGrid() const
Returns a const reference of the grid along the X direction.
Definition: hfield.h:248
static void run(const Vec3f &pointA, const Vec3f &pointB, AABB &bv)
Definition: hfield.h:178
virtual ~HeightField()
deconstruction, delete mesh data related.
Definition: hfield.h:271
Data structure depicting a height field given by the base grid dimensions and the elevation along the...
Definition: hfield.h:194
Vec3f computeCOM() const
compute center of mass
Definition: hfield.h:360
FCL_REAL getMaxHeight() const
Returns the maximal height value of the Height Field.
Definition: hfield.h:263
BVS bvs
Bounding volume hierarchy.
Definition: hfield.h:390
Eigen::Vector2i rightChildIndexes() const
Definition: hfield.h:101
request to the collision algorithm
Definition: collision_data.h:209
HFNode< BV > Node
Definition: hfield.h:201
const VecXf & getYGrid() const
Returns a const reference of the grid along the Y direction.
Definition: hfield.h:250
Eigen::DenseIndex x_size
Definition: hfield.h:64
double FCL_REAL
Definition: data_types.h:66
Definition: collision_object.h:55
Definition: traversal_node_setup.h:851
VecXf y_grid
Definition: hfield.h:387
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
std::vector< Node > BVS
Definition: hfield.h:202
Definition: BVH_internal.h:65
void init(const FCL_REAL x_dim, const FCL_REAL y_dim, const MatrixXf &heights, const FCL_REAL min_height)
Definition: hfield.h:328
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
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:55
NODE_TYPE getNodeType() const
Get the BV type: default is unknown.
Definition: hfield.h:512
Eigen::DenseIndex y_size
Definition: hfield.h:65
const MatrixXf & getHeights() const
Returns a const reference of the heights.
Definition: hfield.h:253
const HFNode< BV > & getBV(unsigned int i) const
Access the bv giving the its index.
Definition: hfield.h:496
HFNodeBase()
Default constructor.
Definition: hfield.h:68
Vec3f getCenter() const
Access to the center of the BV.
Definition: hfield.h:144
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, capsule, cone, cylinder, convex, plane, triangle), and octree
Definition: collision_object.h:58
HFNodeBase Base
Definition: hfield.h:108
VecXf x_grid
Grids along the X and Y directions. Useful for plotting or other related things.
Definition: hfield.h:387
void updateHeights(const MatrixXf &new_heights)
Update Height Field height.
Definition: hfield.h:313
FCL_REAL getMinHeight() const
Returns the minimal height value of the Height Field.
Definition: hfield.h:261
Eigen::DenseIndex y_id
Definition: hfield.h:65
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
CollisionGeometry Base
Definition: hfield.h:199
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: fwd.hh:53
Eigen::Vector2i leftChildIndexes() const
Definition: hfield.h:100
MatrixXf heights
Elevation values in meters of the Height Field.
Definition: hfield.h:381
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
HeightField(const HeightField &other)
Copy contructor from another HeightField.
Definition: hfield.h:232
int buildTree()
Build the bounding volume hierarchy.
Definition: hfield.h:394
static void run(const Vec3f &pointA, const Vec3f &pointB, BV &bv)
Definition: hfield.h:166
const Matrix3f & getOrientation() const
Access to the orientation of the BV.
Definition: hfield.h:147
FCL_REAL x_dim
Dimensions in meters along X and Y directions.
Definition: hfield.h:378
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
HFNode< BV > & getBV(unsigned int i)
Access the bv giving the its index.
Definition: hfield.h:504
The geometry for the object for collision or distance computation.
Definition: collision_object.h:66
FCL_REAL max_height
Definition: hfield.h:384
bool isLeaf() const
Whether current node is a leaf node (i.e. contains a primitive index)
Definition: hfield.h:92
HeightField()
Constructing an empty HeightField.
Definition: hfield.h:205
unsigned int num_bvs
Definition: hfield.h:391
FCL_REAL recursiveUpdateHeight(const size_t bv_id)
Definition: hfield.h:405
Eigen::DenseIndex x_id
Definition: hfield.h:64
#define HPP_FCL_DLLAPI
Definition: config.hh:64
bool overlap(const HFNode &other, const CollisionRequest &request, FCL_REAL &sqrDistLowerBound) const
Check whether two BVNode collide.
Definition: hfield.h:131