Class ConvexMesh

Inheritance Relationships

Base Type

Class Documentation

class ConvexMesh : public bodies::Body

Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.

Public Functions

inline ConvexMesh()
inline ConvexMesh(const shapes::Shape *shape)
~ConvexMesh() override = default
virtual std::vector<double> getDimensions() const override

Returns an empty vector.

virtual std::vector<double> getScaledDimensions() const override

Returns an empty vector.

virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose = false) const override

Check if a point is inside the body. Surface points are included.

virtual double computeVolume() const override

Compute the volume of the body. This method includes changes induced by scaling and padding.

virtual void computeBoundingSphere(BoundingSphere &sphere) const override

Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for.

virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const override

Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted for.

virtual void computeBoundingBox(AABB &bbox) const override

Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are accounted for.

virtual bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections = nullptr, unsigned int count = 0) const override

Check if a ray intersects the body, and find the set of intersections, in order, along the ray. A maximum number of intersections can be specified as well. If that number is 0, all intersections are returned. Passing dir as a unit vector will result in faster computation.

const std::vector<unsigned int> &getTriangles() const
const EigenSTL::vector_Vector3d &getVertices() const
const EigenSTL::vector_Vector3d &getScaledVertices() const
const EigenSTL::vector_Vector4d &getPlanes() const

Get the planes that define the convex shape.

Returns:

A list of Vector4d(nx, ny, nz, d).

virtual BodyPtr cloneAt(const Eigen::Isometry3d &pose, double padding, double scale) const override

Get a clone of this body, but one that is located at the pose pose and has possibly different passing and scaling: padding and scaling. This function is useful to implement thread safety, when bodies need to be moved around.

void computeScaledVerticesFromPlaneProjections()

Project the original vertex to the scaled and padded planes and average.

void correctVertexOrderFromPlanes()
virtual void updateInternalData() override

This function is called every time a change to the body is made, so that intermediate values stored for efficiency reasons are kept up to date.

Protected Functions

virtual void useDimensions(const shapes::Shape *shape) override

Depending on the shape, this function copies the relevant data to the body.

unsigned int countVerticesBehindPlane(const Eigen::Vector4f &planeNormal) const

(Used mainly for debugging) Count the number of vertices behind a plane

bool isPointInsidePlanes(const Eigen::Vector3d &point) const

Check if the point is inside all halfspaces this mesh consists of (mesh_data_->planes_).

Note

The point is expected to have pose_ “cancelled” (have inverse pose of this mesh applied to it).

Note

Scale and padding of the mesh are taken into account.

Note

There is a 1e-9 margin “outside” the planes where points are still considered to be inside.

Protected Attributes

std::shared_ptr<MeshData> mesh_data_
Eigen::Isometry3d i_pose_
Eigen::Vector3d center_
double radiusB_
double radiusBSqr_
Box bounding_box_
EigenSTL::vector_Vector3d *scaled_vertices_