37 #ifndef HPP_FCL_HEIGHT_FIELD_H
38 #define HPP_FCL_HEIGHT_FIELD_H
45 #include <boost/integer/integer_log2.hpp>
64 Eigen::DenseIndex
x_id, x_size;
65 Eigen::DenseIndex
y_id, y_size;
88 return !(*
this == other);
92 inline bool isLeaf()
const {
return x_size == 1 && y_size == 1; }
95 inline size_t leftChild()
const {
return first_child; }
98 inline size_t rightChild()
const {
return first_child + 1; }
101 inline Eigen::Vector2i
rightChildIndexes()
const {
return Eigen::Vector2i(x_id + x_size/2, y_id + y_size/2); }
104 template<
typename BV>
116 return Base::operator==(other) && bv == other.
bv;
122 return !(*
this == other);
128 return bv.overlap(other.
bv);
134 return bv.overlap(other.
bv, request, sqrDistLowerBound);
140 return bv.distance(other.
bv, P1, P2);
149 static const Matrix3f id3 = Matrix3f::Identity();
156 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
163 template<
typename BV>
168 AABB bv_aabb(pointA,pointB);
171 convertBV(bv_aabb,bv);
180 AABB bv_aabb(pointA,pointB);
181 convertBV(bv_aabb,bv);
193 template<
typename BV>
202 typedef std::vector<Node>
BVS;
207 , min_height((std::numeric_limits<
FCL_REAL>::min)())
208 , max_height((std::numeric_limits<
FCL_REAL>::max)())
225 init(x_dim,y_dim,heights,min_height);
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)
242 , num_bvs(other.num_bvs)
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);
280 aabb_radius = (A-B).norm()/2.;
287 bool res = Base::operator==(other);
293 && x_dim == other.
x_dim
294 && y_dim == other.
y_dim
309 return !(*
this == other);
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);
321 heights = new_heights.cwiseMax(min_height);
322 this->max_height = recursiveUpdateHeight(0);
323 assert(this->max_height == heights.maxCoeff());
335 this->heights = heights.cwiseMax(min_height);
336 this->min_height = min_height;
337 this->max_height = heights.maxCoeff();
339 const Eigen::DenseIndex
342 assert(NX >= 2 &&
"The number of columns is too small.");
343 assert(NY >= 2 &&
"The number of rows is too small.");
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);
349 const size_t num_tot_bvs = (size_t)(NX * NY) - 1 + (size_t)((NX-1) * (NY-1));
350 bvs.resize(num_tot_bvs);
362 return Vec3f::Zero();
372 return Matrix3f::Zero();
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");
412 max_height = heights.block<2,2>(bv_node.
y_id,bv_node.
x_id).maxCoeff();
417 max_left_height = recursiveUpdateHeight(bv_node.
leftChild()),
418 max_right_height = recursiveUpdateHeight(bv_node.
rightChild());
420 max_height = (std::max)(max_left_height,max_right_height);
423 const Vec3f pointA(x_grid[bv_node.
x_id],y_grid[bv_node.
y_id],min_height);
432 const Eigen::DenseIndex x_id,
const Eigen::DenseIndex x_size,
433 const Eigen::DenseIndex y_id,
const Eigen::DenseIndex y_size)
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");
442 if(x_size == 1 && y_size == 1)
444 max_height = heights.block<2,2>(y_id,x_id).maxCoeff();
451 FCL_REAL max_left_height = min_height, max_right_height = min_height;
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(),
460 max_right_height = recursiveBuildTree(bvnode.
rightChild(),
461 x_id + x_size_half, x_size - x_size_half,
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(),
472 max_right_height = recursiveBuildTree(bvnode.
rightChild(),
474 y_id + y_size_half, y_size - y_size_half);
477 max_height = (std::max)(max_left_height,max_right_height);
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);
488 bvnode.
x_id = x_id; bvnode.
y_id = y_id;
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
virtual HeightField< BV > * clone() const
Clone *this into a new CollisionGeometry.
Definition: hfield.h:265
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
const VecXf & getXGrid() const
Returns a const reference of the grid along the X direction.
Definition: hfield.h:248
HFNode< BV > & getBV(unsigned int i)
Access the bv giving the its index.
Definition: hfield.h:504
FCL_REAL x_dim
Dimensions in meters along X and Y directions.
Definition: hfield.h:378
VecXf y_grid
Definition: hfield.h:387
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
OBJECT_TYPE getObjectType() const
Get the object type: it is a HFIELD.
Definition: hfield.h:358
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
const HFNode< BV > & getBV(unsigned int i) const
Access the bv giving the its index.
Definition: hfield.h:496
FCL_REAL recursiveUpdateHeight(const size_t bv_id)
Definition: hfield.h:405
const VecXf & getYGrid() const
Returns a const reference of the grid along the Y direction.
Definition: hfield.h:250
const MatrixXf & getHeights() const
Returns a const reference of the heights.
Definition: hfield.h:253
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
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
Definition: traversal_node_setup.h:852
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
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
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