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 <vector>
46 
47 namespace hpp {
48 namespace fcl {
49 
52 
53 struct HPP_FCL_DLLAPI HFNodeBase {
58  size_t first_child;
59 
60  Eigen::DenseIndex x_id, x_size;
61  Eigen::DenseIndex y_id, y_size;
62 
64 
67  : first_child(0),
68  x_id(-1),
69  x_size(0),
70  y_id(-1),
71  y_size(0),
72  max_height(std::numeric_limits<FCL_REAL>::lowest()) {}
73 
75  bool operator==(const HFNodeBase& other) const {
76  return first_child == other.first_child && x_id == other.x_id &&
77  x_size == other.x_size && y_id == other.y_id &&
78  y_size == other.y_size && max_height == other.max_height;
79  }
80 
82  bool operator!=(const HFNodeBase& other) const { return !(*this == other); }
83 
86  inline bool isLeaf() const { return x_size == 1 && y_size == 1; }
87 
90  inline size_t leftChild() const { return first_child; }
91 
94  inline size_t rightChild() const { return first_child + 1; }
95 
96  inline Eigen::Vector2i leftChildIndexes() const {
97  return Eigen::Vector2i(x_id, y_id);
98  }
99  inline Eigen::Vector2i rightChildIndexes() const {
100  return Eigen::Vector2i(x_id + x_size / 2, y_id + y_size / 2);
101  }
102 };
103 
104 template <typename BV>
105 struct HPP_FCL_DLLAPI HFNode : public HFNodeBase {
106  typedef HFNodeBase Base;
107 
109  BV bv;
110 
112  bool operator==(const HFNode& other) const {
113  return Base::operator==(other) && bv == other.bv;
114  }
115 
117  bool operator!=(const HFNode& other) const { return !(*this == other); }
118 
120  bool overlap(const HFNode& other) const { return bv.overlap(other.bv); }
122  bool overlap(const HFNode& other, const CollisionRequest& request,
123  FCL_REAL& sqrDistLowerBound) const {
124  return bv.overlap(other.bv, request, sqrDistLowerBound);
125  }
126 
129  FCL_REAL distance(const HFNode& other, Vec3f* P1 = NULL,
130  Vec3f* P2 = NULL) const {
131  return bv.distance(other.bv, P1, P2);
132  }
133 
135  Vec3f getCenter() const { return bv.center(); }
136 
138  const Matrix3f& getOrientation() const {
139  static const Matrix3f id3 = Matrix3f::Identity();
140  return id3;
141  }
142 
143  virtual ~HFNode() {}
144 
146  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
148 };
149 
150 namespace details {
151 
152 template <typename BV>
154  static void run(const Vec3f& pointA, const Vec3f& pointB, BV& bv) {
155  AABB bv_aabb(pointA, pointB);
156  // AABB bv_aabb;
157  // bv_aabb.update(pointA,pointB);
158  convertBV(bv_aabb, bv);
159  }
160 };
161 
162 template <>
164  static void run(const Vec3f& pointA, const Vec3f& pointB, AABB& bv) {
165  AABB bv_aabb(pointA, pointB);
166  convertBV(bv_aabb, bv);
167  // bv.update(pointA,pointB);
168  }
169 };
170 
171 } // namespace details
172 
181 template <typename BV>
182 class HPP_FCL_DLLAPI HeightField : public CollisionGeometry {
183  public:
185 
186  typedef HFNode<BV> Node;
187  typedef std::vector<Node> BVS;
188 
191  : CollisionGeometry(),
192  min_height((std::numeric_limits<FCL_REAL>::min)()),
193  max_height((std::numeric_limits<FCL_REAL>::max)()) {}
194 
206  HeightField(const FCL_REAL x_dim, const FCL_REAL y_dim,
207  const MatrixXf& heights, const FCL_REAL min_height = (FCL_REAL)0)
208  : CollisionGeometry() {
209  init(x_dim, y_dim, heights, min_height);
210  }
211 
216  HeightField(const HeightField& other)
217  : CollisionGeometry(other),
218  x_dim(other.x_dim),
219  y_dim(other.y_dim),
220  heights(other.heights),
221  min_height(other.min_height),
222  max_height(other.max_height),
223  x_grid(other.x_grid),
224  y_grid(other.y_grid),
225  bvs(other.bvs),
226  num_bvs(other.num_bvs) {}
227 
229  const VecXf& getXGrid() const { return x_grid; }
231  const VecXf& getYGrid() const { return y_grid; }
232 
234  const MatrixXf& getHeights() const { return heights; }
235 
237  FCL_REAL getXDim() const { return x_dim; }
239  FCL_REAL getYDim() const { return y_dim; }
240 
242  FCL_REAL getMinHeight() const { return min_height; }
244  FCL_REAL getMaxHeight() const { return max_height; }
245 
246  virtual HeightField<BV>* clone() const { return new HeightField(*this); }
247 
249  virtual ~HeightField() {}
250 
254  const Vec3f A(x_grid[0], y_grid[0], min_height);
255  const Vec3f B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1],
256  max_height);
257  const AABB aabb_(A, B);
258 
259  aabb_radius = (A - B).norm() / 2.;
260  aabb_local = aabb_;
261  aabb_center = aabb_.center();
262  }
263 
265  void updateHeights(const MatrixXf& new_heights) {
266  if (new_heights.rows() != heights.rows() ||
267  new_heights.cols() != heights.cols())
269  "The matrix containing the new heights values does not have the same "
270  "matrix size as the original one.\n"
271  "\tinput values - rows: "
272  << new_heights.rows() << " - cols: " << new_heights.cols() << "\n"
273  << "\texpected values - rows: " << heights.rows()
274  << " - cols: " << heights.cols() << "\n",
275  std::invalid_argument);
276 
277  heights = new_heights.cwiseMax(min_height);
278  this->max_height = recursiveUpdateHeight(0);
279  assert(this->max_height == heights.maxCoeff());
280  }
281 
282  protected:
283  void init(const FCL_REAL x_dim, const FCL_REAL y_dim, const MatrixXf& heights,
284  const FCL_REAL min_height) {
285  this->x_dim = x_dim;
286  this->y_dim = y_dim;
287  this->heights = heights.cwiseMax(min_height);
288  this->min_height = min_height;
289  this->max_height = heights.maxCoeff();
290 
291  const Eigen::DenseIndex NX = heights.cols(), NY = heights.rows();
292  assert(NX >= 2 && "The number of columns is too small.");
293  assert(NY >= 2 && "The number of rows is too small.");
294 
295  x_grid = VecXf::LinSpaced(NX, -0.5 * x_dim, 0.5 * x_dim);
296  y_grid = VecXf::LinSpaced(NY, 0.5 * y_dim, -0.5 * y_dim);
297 
298  // Allocate BVS
299  const size_t num_tot_bvs =
300  (size_t)(NX * NY) - 1 + (size_t)((NX - 1) * (NY - 1));
301  bvs.resize(num_tot_bvs);
302  num_bvs = 0;
303 
304  // Build tree
305  buildTree();
306  }
307 
309  OBJECT_TYPE getObjectType() const { return OT_HFIELD; }
310 
311  Vec3f computeCOM() const { return Vec3f::Zero(); }
312 
313  FCL_REAL computeVolume() const { return 0; }
314 
315  Matrix3f computeMomentofInertia() const { return Matrix3f::Zero(); }
316 
317  protected:
319  FCL_REAL x_dim, y_dim;
320 
323 
326  FCL_REAL min_height, max_height;
327 
330  VecXf x_grid, y_grid;
331 
333  BVS bvs;
334  unsigned int num_bvs;
335 
337  int buildTree() {
338  num_bvs = 1;
339  const FCL_REAL max_recursive_height =
340  recursiveBuildTree(0, 0, heights.cols() - 1, 0, heights.rows() - 1);
341  assert(max_recursive_height == max_height &&
342  "the maximal height is not correct");
343  HPP_FCL_UNUSED_VARIABLE(max_recursive_height);
344 
345  bvs.resize(num_bvs);
346  return BVH_OK;
347  }
348 
349  FCL_REAL recursiveUpdateHeight(const size_t bv_id) {
350  HFNode<BV>& bv_node = bvs[bv_id];
351 
352  FCL_REAL max_height;
353  if (bv_node.isLeaf()) {
354  max_height = heights.block<2, 2>(bv_node.y_id, bv_node.x_id).maxCoeff();
355  } else {
356  FCL_REAL
357  max_left_height = recursiveUpdateHeight(bv_node.leftChild()),
358  max_right_height = recursiveUpdateHeight(bv_node.rightChild());
359 
360  max_height = (std::max)(max_left_height, max_right_height);
361  }
362 
363  bv_node.max_height = max_height;
364 
365  const Vec3f pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height);
366  const Vec3f pointB(x_grid[bv_node.x_id + bv_node.x_size],
367  y_grid[bv_node.y_id + bv_node.y_size], max_height);
368 
369  details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
370 
371  return max_height;
372  }
373 
374  FCL_REAL recursiveBuildTree(const size_t bv_id, const Eigen::DenseIndex x_id,
375  const Eigen::DenseIndex x_size,
376  const Eigen::DenseIndex y_id,
377  const Eigen::DenseIndex y_size) {
378  assert(x_id < heights.cols() && "x_id is out of bounds");
379  assert(y_id < heights.rows() && "y_id is out of bounds");
380  assert(x_size >= 0 && y_size >= 0 &&
381  "x_size or y_size are not of correct value");
382  assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension");
383 
384  HFNode<BV>& bv_node = bvs[bv_id];
385  FCL_REAL max_height;
386  if (x_size == 1 &&
387  y_size == 1) // don't build any BV for the current child node
388  {
389  max_height = heights.block<2, 2>(y_id, x_id).maxCoeff();
390  } else {
391  bv_node.first_child = num_bvs;
392  num_bvs += 2;
393 
394  FCL_REAL max_left_height = min_height, max_right_height = min_height;
395  if (x_size >= y_size) // splitting along the X axis
396  {
397  Eigen::DenseIndex x_size_half = x_size / 2;
398  if (x_size == 1) x_size_half = 1;
399  max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id,
400  x_size_half, y_id, y_size);
401 
402  max_right_height =
403  recursiveBuildTree(bv_node.rightChild(), x_id + x_size_half,
404  x_size - x_size_half, y_id, y_size);
405  } else // splitting along the Y axis
406  {
407  Eigen::DenseIndex y_size_half = y_size / 2;
408  if (y_size == 1) y_size_half = 1;
409  max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size,
410  y_id, y_size_half);
411 
412  max_right_height =
413  recursiveBuildTree(bv_node.rightChild(), x_id, x_size,
414  y_id + y_size_half, y_size - y_size_half);
415  }
416 
417  max_height = (std::max)(max_left_height, max_right_height);
418  }
419 
420  bv_node.max_height = max_height;
421  // max_height = std::max(max_height,min_height);
422 
423  const Vec3f pointA(x_grid[x_id], y_grid[y_id], min_height);
424  assert(x_id + x_size < x_grid.size());
425  assert(y_id + y_size < y_grid.size());
426  const Vec3f pointB(x_grid[x_id + x_size], y_grid[y_id + y_size],
427  max_height);
428 
429  details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
430  bv_node.x_id = x_id;
431  bv_node.y_id = y_id;
432  bv_node.x_size = x_size;
433  bv_node.y_size = y_size;
434 
435  return max_height;
436  }
437 
438  public:
440  const HFNode<BV>& getBV(unsigned int i) const {
441  if (i >= num_bvs)
442  HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
443  return bvs[i];
444  }
445 
447  HFNode<BV>& getBV(unsigned int i) {
448  if (i >= num_bvs)
449  HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument);
450  return bvs[i];
451  }
452 
454  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
455 
456  private:
457  virtual bool isEqual(const CollisionGeometry& _other) const {
458  const HeightField* other_ptr = dynamic_cast<const HeightField*>(&_other);
459  if (other_ptr == nullptr) return false;
460  const HeightField& other = *other_ptr;
461 
462  return x_dim == other.x_dim && y_dim == other.y_dim &&
463  heights == other.heights && min_height == other.min_height &&
464  max_height == other.max_height && x_grid == other.x_grid &&
465  y_grid == other.y_grid && bvs == other.bvs &&
466  num_bvs == other.num_bvs;
467  }
468 
469  public:
470  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
471 };
472 
475 template <>
477 
478 template <>
480 
481 template <>
483 
484 template <>
486 
487 template <>
489 
490 template <>
491 NODE_TYPE HeightField<KDOP<16> >::getNodeType() const;
492 
493 template <>
494 NODE_TYPE HeightField<KDOP<18> >::getNodeType() const;
495 
496 template <>
497 NODE_TYPE HeightField<KDOP<24> >::getNodeType() const;
498 
500 
501 } // namespace fcl
502 
503 } // namespace hpp
504 
505 #endif
bool overlap(const HFNode &other) const
Check whether two BVNode collide.
Definition: hfield.h:120
FCL_REAL getYDim() const
Returns the dimension of the Height Field along the Y direction.
Definition: hfield.h:239
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, Eigen::Dynamic > MatrixXf
Definition: data_types.h:71
tuple P1
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: hfield.h:315
bool operator!=(const HFNodeBase &other) const
Difference operator.
Definition: hfield.h:82
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:206
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
OBJECT_TYPE getObjectType() const
Get the object type: it is a HFIELD.
Definition: hfield.h:309
Main namespace.
virtual bool isEqual(const CollisionGeometry &_other) const
equal operator with another object of derived type.
Definition: hfield.h:457
virtual HeightField< BV > * clone() const
Clone *this into a new CollisionGeometry.
Definition: hfield.h:246
FCL_REAL min_height
Minimal height of the Height Field: all values bellow min_height will be discarded.
Definition: hfield.h:326
bool operator==(const HFNode &other) const
Equality operator.
Definition: hfield.h:112
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, 1 > VecXf
Definition: data_types.h:67
bool operator!=(const HFNode &other) const
Difference operator.
Definition: hfield.h:117
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:129
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
FCL_REAL getXDim() const
Returns the dimension of the Height Field along the X direction.
Definition: hfield.h:237
FCL_REAL computeVolume() const
compute the volume
Definition: hfield.h:313
bool operator==(const HFNodeBase &other) const
Comparison operator.
Definition: hfield.h:75
void computeLocalAABB()
Compute the AABB for the HeightField, used for broad-phase collision.
Definition: hfield.h:253
BV bv
bounding volume storing the geometry
Definition: hfield.h:109
virtual ~HFNode()
Definition: hfield.h:143
const VecXf & getXGrid() const
Returns a const reference of the grid along the X direction.
Definition: hfield.h:229
static void run(const Vec3f &pointA, const Vec3f &pointB, AABB &bv)
Definition: hfield.h:164
virtual ~HeightField()
deconstruction, delete mesh data related.
Definition: hfield.h:249
Data structure depicting a height field given by the base grid dimensions and the elevation along the...
Definition: hfield.h:182
Vec3f computeCOM() const
compute center of mass
Definition: hfield.h:311
FCL_REAL getMaxHeight() const
Returns the maximal height value of the Height Field.
Definition: hfield.h:244
BVS bvs
Bounding volume hierarchy.
Definition: hfield.h:333
Eigen::Vector2i rightChildIndexes() const
Definition: hfield.h:99
request to the collision algorithm
HFNode< BV > Node
Definition: hfield.h:186
const VecXf & getYGrid() const
Returns a const reference of the grid along the Y direction.
Definition: hfield.h:231
Eigen::DenseIndex x_size
Definition: hfield.h:60
FCL_REAL max_height
Definition: hfield.h:63
#define HPP_FCL_UNUSED_VARIABLE(var)
double FCL_REAL
Definition: data_types.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:58
std::vector< Node > BVS
Definition: hfield.h:187
void init(const FCL_REAL x_dim, const FCL_REAL y_dim, const MatrixXf &heights, const FCL_REAL min_height)
Definition: hfield.h:283
size_t leftChild() const
Return the index of the first child. The index is referred to the bounding volume array (i...
Definition: hfield.h:90
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
NODE_TYPE getNodeType() const
Get the BV type: default is unknown.
Definition: hfield.h:454
Eigen::DenseIndex y_size
Definition: hfield.h:61
const MatrixXf & getHeights() const
Returns a const reference of the heights.
Definition: hfield.h:234
const HFNode< BV > & getBV(unsigned int i) const
Access the bv giving the its index.
Definition: hfield.h:440
HFNodeBase()
Default constructor.
Definition: hfield.h:66
Vec3f getCenter() const
Access to the center of the BV.
Definition: hfield.h:135
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, cylinder, convex, plane, triangle), and octree
HFNodeBase Base
Definition: hfield.h:106
VecXf x_grid
Grids along the X and Y directions. Useful for plotting or other related things.
Definition: hfield.h:330
void updateHeights(const MatrixXf &new_heights)
Update Height Field height.
Definition: hfield.h:265
B
FCL_REAL getMinHeight() const
Returns the minimal height value of the Height Field.
Definition: hfield.h:242
Eigen::DenseIndex y_id
Definition: hfield.h:61
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:374
CollisionGeometry Base
Definition: hfield.h:184
Eigen::Vector2i leftChildIndexes() const
Definition: hfield.h:96
MatrixXf heights
Elevation values in meters of the Height Field.
Definition: hfield.h:322
tuple P2
size_t rightChild() const
Return the index of the second child. The index is referred to the bounding volume array (i...
Definition: hfield.h:94
HeightField(const HeightField &other)
Copy contructor from another HeightField.
Definition: hfield.h:216
int buildTree()
Build the bounding volume hierarchy.
Definition: hfield.h:337
static void run(const Vec3f &pointA, const Vec3f &pointB, BV &bv)
Definition: hfield.h:154
const Matrix3f & getOrientation() const
Access to the orientation of the BV.
Definition: hfield.h:138
FCL_REAL x_dim
Dimensions in meters along X and Y directions.
Definition: hfield.h:319
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
static void convertBV(const BV1 &bv1, const Transform3f &tf1, BV2 &bv2)
Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity...
Definition: BV.h:277
A
HFNode< BV > & getBV(unsigned int i)
Access the bv giving the its index.
Definition: hfield.h:447
The geometry for the object for collision or distance computation.
FCL_REAL max_height
Definition: hfield.h:326
bool isLeaf() const
Whether current node is a leaf node (i.e. contains a primitive index)
Definition: hfield.h:86
HeightField()
Constructing an empty HeightField.
Definition: hfield.h:190
unsigned int num_bvs
Definition: hfield.h:334
FCL_REAL recursiveUpdateHeight(const size_t bv_id)
Definition: hfield.h:349
Eigen::DenseIndex x_id
Definition: hfield.h:60
#define HPP_FCL_THROW_PRETTY(message, exception)
bool overlap(const HFNode &other, const CollisionRequest &request, FCL_REAL &sqrDistLowerBound) const
Check whether two BVNode collide.
Definition: hfield.h:122


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:01