33 #ifndef DART_DYNAMICS_VOXELGRIDSHAPE_HPP_
34 #define DART_DYNAMICS_VOXELGRIDSHAPE_HPP_
36 #include "dart/config.hpp"
40 # include <octomap/octomap.h>
49 class VoxelGridShape :
public Shape
54 explicit VoxelGridShape(
double resolution = 0.01);
61 ~VoxelGridShape()
override =
default;
64 const std::string& getType()
const override;
67 static const std::string& getStaticType();
85 void updateOccupancy(
const Eigen::Vector3d& point,
bool occupied =
true);
98 void updateOccupancy(
const Eigen::Vector3d& from,
const Eigen::Vector3d& to);
115 void updateOccupancy(
116 const octomap::Pointcloud& pointCloud,
117 const Eigen::Vector3d& sensorOrigin = Eigen::Vector3d::Zero(),
135 void updateOccupancy(
136 const octomap::Pointcloud& pointCloud,
137 const Eigen::Vector3d& sensorOrigin,
138 const Eigen::Isometry3d& relativeTo);
141 double getOccupancy(
const Eigen::Vector3d& point)
const;
144 Eigen::Matrix3d computeInertia(
double mass)
const override;
147 void notifyColorUpdated(
const Eigen::Vector4d& color)
override;
151 void updateBoundingBox()
const override;
154 void updateVolume()
const override;
static Frame * World()
Definition: Frame.cpp:73
boost::shared_ptr< T > fcl_shared_ptr
Definition: BackwardCompatibility.hpp:106
Definition: BulletCollisionDetector.cpp:65