Template Class HeightField

Inheritance Relationships

Base Type

Derived Type

Class Documentation

template<typename BV>
class HeightField : public hpp::fcl::CollisionGeometry

Data structure depicting a height field given by the base grid dimensions and the elevation along the grid.

An height field is defined by its base dimensions along the X and Y axes and a set ofpoints defined by their altitude, regularly dispatched on the grid. The height field is centered at the origin and the corners of the geometry correspond to the following coordinates [± x_dim/2; ± y_dim/2].

Template Parameters:

BV – one of the bounding volume class in Bounding volumes.

Subclassed by boost::serialization::internal::HeightFieldAccessor< BV >

Public Types

typedef CollisionGeometry Base
typedef HFNode<BV> Node
typedef std::vector<Node> BVS

Public Functions

inline HeightField()

Constructing an empty HeightField.

inline 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.

Parameters:
  • x_dim[in] Dimension along the X axis

  • y_dim[in] Dimension along the Y axis

  • heights[in] Matrix containing the altitude of each point compositng the height field

  • min_height[in] Minimal height of the height field

inline HeightField(const HeightField &other)

Copy contructor from another HeightField.

Parameters:

other[in] to copy.

inline const VecXf &getXGrid() const

Returns a const reference of the grid along the X direction.

inline const VecXf &getYGrid() const

Returns a const reference of the grid along the Y direction.

inline const MatrixXf &getHeights() const

Returns a const reference of the heights.

inline FCL_REAL getXDim() const

Returns the dimension of the Height Field along the X direction.

inline FCL_REAL getYDim() const

Returns the dimension of the Height Field along the Y direction.

inline FCL_REAL getMinHeight() const

Returns the minimal height value of the Height Field.

inline FCL_REAL getMaxHeight() const

Returns the maximal height value of the Height Field.

inline virtual HeightField<BV> *clone() const

Clone *this into a new CollisionGeometry.

inline virtual ~HeightField()

deconstruction, delete mesh data related.

inline virtual void computeLocalAABB()

Compute the AABB for the HeightField, used for broad-phase collision.

inline void updateHeights(const MatrixXf &new_heights)

Update Height Field height.

inline const HFNode<BV> &getBV(unsigned int i) const

Access the bv giving the its index.

inline HFNode<BV> &getBV(unsigned int i)

Access the bv giving the its index.

inline virtual NODE_TYPE getNodeType() const

Get the BV type: default is unknown.

virtual NODE_TYPE getNodeType() const

Specialization of getNodeType() for HeightField with different BV types.

virtual NODE_TYPE getNodeType() const

get the node type

virtual NODE_TYPE getNodeType() const

get the node type

virtual NODE_TYPE getNodeType() const

get the node type

virtual NODE_TYPE getNodeType() const

get the node type

virtual NODE_TYPE getNodeType() const

get the node type

virtual NODE_TYPE getNodeType() const

get the node type

virtual NODE_TYPE getNodeType() const

get the node type

Protected Functions

inline void init(const FCL_REAL x_dim, const FCL_REAL y_dim, const MatrixXf &heights, const FCL_REAL min_height)
inline virtual OBJECT_TYPE getObjectType() const

Get the object type: it is a HFIELD.

inline virtual Vec3f computeCOM() const

compute center of mass

inline virtual FCL_REAL computeVolume() const

compute the volume

inline virtual Matrix3f computeMomentofInertia() const

compute the inertia matrix, related to the origin

inline int buildTree()

Build the bounding volume hierarchy.

inline FCL_REAL recursiveUpdateHeight(const size_t bv_id)
inline 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)

Protected Attributes

FCL_REAL x_dim

Dimensions in meters along X and Y directions.

FCL_REAL y_dim
MatrixXf heights

Elevation values in meters of the Height Field.

FCL_REAL min_height

Minimal height of the Height Field: all values bellow min_height will be discarded.

FCL_REAL max_height
VecXf x_grid

Grids along the X and Y directions. Useful for plotting or other related things.

VecXf y_grid
BVS bvs

Bounding volume hierarchy.

unsigned int num_bvs