hpp-fcl 2.4.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 <vector>
46
47namespace hpp {
48namespace fcl {
49
52
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
104template <typename BV>
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
150namespace details {
151
152template <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
162template <>
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
181template <typename BV>
183 public:
185
187 typedef std::vector<Node> BVS;
188
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)
209 init(x_dim, y_dim, heights, min_height);
210 }
211
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
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:
320
323
326 FCL_REAL min_height, max_height;
327
330 VecXf x_grid, y_grid;
331
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 {
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
475template <>
477
478template <>
480
481template <>
483
484template <>
486
487template <>
489
490template <>
491NODE_TYPE HeightField<KDOP<16> >::getNodeType() const;
492
493template <>
494NODE_TYPE HeightField<KDOP<18> >::getNodeType() const;
495
496template <>
497NODE_TYPE HeightField<KDOP<24> >::getNodeType() const;
498
500
501} // namespace fcl
502
503} // namespace hpp
504
505#endif
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:54
The geometry for the object for collision or distance computation.
Definition: collision_object.h:95
Data structure depicting a height field given by the base grid dimensions and the elevation along the...
Definition: hfield.h:182
FCL_REAL max_height
Definition: hfield.h:326
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
FCL_REAL y_dim
Definition: hfield.h:319
void init(const FCL_REAL x_dim, const FCL_REAL y_dim, const MatrixXf &heights, const FCL_REAL min_height)
Definition: hfield.h:283
FCL_REAL computeVolume() const
compute the volume
Definition: hfield.h:313
FCL_REAL min_height
Minimal height of the Height Field: all values bellow min_height will be discarded.
Definition: hfield.h:326
FCL_REAL x_dim
Dimensions in meters along X and Y directions.
Definition: hfield.h:319
const HFNode< BV > & getBV(unsigned int i) const
Access the bv giving the its index.
Definition: hfield.h:440
VecXf y_grid
Definition: hfield.h:330
virtual HeightField< BV > * clone() const
Clone *this into a new CollisionGeometry.
Definition: hfield.h:246
Vec3f computeCOM() const
compute center of mass
Definition: hfield.h:311
FCL_REAL getXDim() const
Returns the dimension of the Height Field along the X direction.
Definition: hfield.h:237
const VecXf & getYGrid() const
Returns a const reference of the grid along the Y direction.
Definition: hfield.h:231
OBJECT_TYPE getObjectType() const
Get the object type: it is a HFIELD.
Definition: hfield.h:309
HFNode< BV > & getBV(unsigned int i)
Access the bv giving the its index.
Definition: hfield.h:447
FCL_REAL getYDim() const
Returns the dimension of the Height Field along the Y direction.
Definition: hfield.h:239
int buildTree()
Build the bounding volume hierarchy.
Definition: hfield.h:337
FCL_REAL getMinHeight() const
Returns the minimal height value of the Height Field.
Definition: hfield.h:242
HeightField(const HeightField &other)
Copy contructor from another HeightField.
Definition: hfield.h:216
HFNode< BV > Node
Definition: hfield.h:186
CollisionGeometry Base
Definition: hfield.h:184
BVS bvs
Bounding volume hierarchy.
Definition: hfield.h:333
FCL_REAL recursiveUpdateHeight(const size_t bv_id)
Definition: hfield.h:349
const VecXf & getXGrid() const
Returns a const reference of the grid along the X direction.
Definition: hfield.h:229
VecXf x_grid
Grids along the X and Y directions. Useful for plotting or other related things.
Definition: hfield.h:330
void computeLocalAABB()
Compute the AABB for the HeightField, used for broad-phase collision.
Definition: hfield.h:253
virtual ~HeightField()
deconstruction, delete mesh data related.
Definition: hfield.h:249
const MatrixXf & getHeights() const
Returns a const reference of the heights.
Definition: hfield.h:234
std::vector< Node > BVS
Definition: hfield.h:187
NODE_TYPE getNodeType() const
Get the BV type: default is unknown.
Definition: hfield.h:454
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:206
unsigned int num_bvs
Definition: hfield.h:334
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: hfield.h:315
HeightField()
Constructing an empty HeightField.
Definition: hfield.h:190
MatrixXf heights
Elevation values in meters of the Height Field.
Definition: hfield.h:322
void updateHeights(const MatrixXf &new_heights)
Update Height Field height.
Definition: hfield.h:265
FCL_REAL getMaxHeight() const
Returns the maximal height value of the Height Field.
Definition: hfield.h:244
#define HPP_FCL_DLLAPI
Definition: config.hh:88
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: fwd.hh:57
#define HPP_FCL_UNUSED_VARIABLE(var)
Definition: fwd.hh:55
Vec3f center() const
Center of the AABB.
Definition: AABB.h:157
@ OT_HFIELD
Definition: collision_object.h:58
@ BV_UNKNOWN
Definition: collision_object.h:66
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, Eigen::Dynamic > MatrixXf
Definition: data_types.h:71
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:53
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, 1 > VecXf
Definition: data_types.h:67
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:65
bool isEqual(const Eigen::MatrixBase< Derived > &lhs, const Eigen::MatrixBase< OtherDerived > &rhs, const FCL_REAL tol=std::numeric_limits< FCL_REAL >::epsilon() *100)
Definition: tools.h:209
double FCL_REAL
Definition: data_types.h:65
@ BVH_OK
Definition: BVH_internal.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44
request to the collision algorithm
Definition: collision_data.h:235
Definition: hfield.h:53
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
FCL_REAL max_height
Definition: hfield.h:63
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
Eigen::Vector2i leftChildIndexes() const
Definition: hfield.h:96
Eigen::DenseIndex y_size
Definition: hfield.h:61
bool operator!=(const HFNodeBase &other) const
Difference operator.
Definition: hfield.h:82
Eigen::DenseIndex x_id
Definition: hfield.h:60
bool isLeaf() const
Whether current node is a leaf node (i.e. contains a primitive index)
Definition: hfield.h:86
Eigen::DenseIndex x_size
Definition: hfield.h:60
Eigen::DenseIndex y_id
Definition: hfield.h:61
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
bool operator==(const HFNodeBase &other) const
Comparison operator.
Definition: hfield.h:75
HFNodeBase()
Default constructor.
Definition: hfield.h:66
Eigen::Vector2i rightChildIndexes() const
Definition: hfield.h:99
Definition: hfield.h:105
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
bool overlap(const HFNode &other) const
Check whether two BVNode collide.
Definition: hfield.h:120
Vec3f getCenter() const
Access to the center of the BV.
Definition: hfield.h:135
bool overlap(const HFNode &other, const CollisionRequest &request, FCL_REAL &sqrDistLowerBound) const
Check whether two BVNode collide.
Definition: hfield.h:122
bool operator!=(const HFNode &other) const
Difference operator.
Definition: hfield.h:117
HFNodeBase Base
Definition: hfield.h:106
const Matrix3f & getOrientation() const
Access to the orientation of the BV.
Definition: hfield.h:138
bool operator==(const HFNode &other) const
Equality operator.
Definition: hfield.h:112
virtual ~HFNode()
Definition: hfield.h:143
BV bv
bounding volume storing the geometry
Definition: hfield.h:109
static void run(const Vec3f &pointA, const Vec3f &pointB, AABB &bv)
Definition: hfield.h:164
static void run(const Vec3f &pointA, const Vec3f &pointB, BV &bv)
Definition: hfield.h:154