A vector of Body objects. More...
#include <bodies.h>
Public Member Functions | |
void | addBody (Body *body) |
Add a body. More... | |
void | addBody (const shapes::Shape *shape, const Eigen::Affine3d &pose, double padding=0.0) |
Add a body from a shape, a pose for the body and a padding. More... | |
BodyVector () | |
BodyVector (const std::vector< shapes::Shape * > &shapes, const EigenSTL::vector_Affine3d &poses, double padding=0.0) | |
Construct a body vector from a vector of shapes, a vector of poses and a padding. More... | |
void | clear () |
Clear all bodies from the vector. More... | |
bool | containsPoint (const Eigen::Vector3d &p, bool verbose=false) const |
Check if any body in the vector contains the input point. More... | |
bool | containsPoint (const Eigen::Vector3d &p, std::size_t &index, bool verbose=false) const |
Check if any body contains the input point, and report the first body's index if so. More... | |
const Body * | getBody (unsigned int i) const |
Get the ith body in the vector. More... | |
std::size_t | getCount () const |
Get the number of bodies in this vector. More... | |
bool | intersectsRay (const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, std::size_t &index, EigenSTL::vector_Vector3d *intersections=NULL, unsigned int count=0) const |
Check if any of the bodies intersects the ray defined by origin and dir. When the first intersection is found, this function terminates. The index of the body that does intersect the ray is set to index (unset if no intersections were found). Optionally, the intersection points are computed and set to intersections (only for the first body that is found to intersect the ray) More... | |
void | setPose (unsigned int i, const Eigen::Affine3d &pose) |
Set the pose of a particular body in the vector of bodies. More... | |
~BodyVector () | |
Private Attributes | |
std::vector< Body * > | bodies_ |
bodies::BodyVector::BodyVector | ( | ) |
Definition at line 1162 of file bodies.cpp.
bodies::BodyVector::BodyVector | ( | const std::vector< shapes::Shape * > & | shapes, |
const EigenSTL::vector_Affine3d & | poses, | ||
double | padding = 0.0 |
||
) |
Construct a body vector from a vector of shapes, a vector of poses and a padding.
Definition at line 1166 of file bodies.cpp.
bodies::BodyVector::~BodyVector | ( | ) |
Definition at line 1173 of file bodies.cpp.
void bodies::BodyVector::addBody | ( | Body * | body | ) |
Add a body.
Definition at line 1185 of file bodies.cpp.
void bodies::BodyVector::addBody | ( | const shapes::Shape * | shape, |
const Eigen::Affine3d & | pose, | ||
double | padding = 0.0 |
||
) |
Add a body from a shape, a pose for the body and a padding.
Definition at line 1192 of file bodies.cpp.
void bodies::BodyVector::clear | ( | ) |
Clear all bodies from the vector.
Definition at line 1178 of file bodies.cpp.
bool bodies::BodyVector::containsPoint | ( | const Eigen::Vector3d & | p, |
bool | verbose = false |
||
) | const |
Check if any body in the vector contains the input point.
Definition at line 1238 of file bodies.cpp.
bool bodies::BodyVector::containsPoint | ( | const Eigen::Vector3d & | p, |
std::size_t & | index, | ||
bool | verbose = false |
||
) | const |
Check if any body contains the input point, and report the first body's index if so.
Definition at line 1227 of file bodies.cpp.
const bodies::Body * bodies::BodyVector::getBody | ( | unsigned int | i | ) | const |
Get the ith body in the vector.
Definition at line 1216 of file bodies.cpp.
std::size_t bodies::BodyVector::getCount | ( | ) | const |
Get the number of bodies in this vector.
Definition at line 1200 of file bodies.cpp.
bool bodies::BodyVector::intersectsRay | ( | const Eigen::Vector3d & | origin, |
const Eigen::Vector3d & | dir, | ||
std::size_t & | index, | ||
EigenSTL::vector_Vector3d * | intersections = NULL , |
||
unsigned int | count = 0 |
||
) | const |
Check if any of the bodies intersects the ray defined by origin and dir. When the first intersection is found, this function terminates. The index of the body that does intersect the ray is set to index (unset if no intersections were found). Optionally, the intersection points are computed and set to intersections (only for the first body that is found to intersect the ray)
Definition at line 1244 of file bodies.cpp.
void bodies::BodyVector::setPose | ( | unsigned int | i, |
const Eigen::Affine3d & | pose | ||
) |
Set the pose of a particular body in the vector of bodies.
Definition at line 1205 of file bodies.cpp.