#include <collision_distance_field_types.h>
Public Member Functions | |
bool | getCollisionSphereGradients (const std::vector< CollisionSphere > &sphere_list, const EigenSTL::vector_Vector3d &sphere_centers, GradientInfo &gradient, const CollisionType &type, double tolerance, bool subtract_radii, double maximum_value, bool stop_at_first_collision) |
double | getDistanceGradient (double x, double y, double z, double &gradient_x, double &gradient_y, double &gradient_z, bool &in_bounds) const |
Gets not only the distance to the nearest cell but the gradient direction. The point defined by the x, y and z values is transform into the local distance field coordinate system using the current pose. The gradient is computed as a function of the distances of near-by cells. An uninitialized distance is returned if the cell is not valid for gradient production purposes. | |
const Eigen::Affine3d & | getPose () const |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | PosedDistanceField (const Eigen::Vector3d &size, const Eigen::Vector3d &origin, double resolution, double max_distance, bool propagate_negative_distances=false) |
void | updatePose (const Eigen::Affine3d &transform) |
Protected Attributes | |
Eigen::Affine3d | pose_ |
Definition at line 107 of file collision_distance_field_types.h.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW collision_detection::PosedDistanceField::PosedDistanceField | ( | const Eigen::Vector3d & | size, |
const Eigen::Vector3d & | origin, | ||
double | resolution, | ||
double | max_distance, | ||
bool | propagate_negative_distances = false |
||
) | [inline] |
Definition at line 112 of file collision_distance_field_types.h.
bool collision_detection::PosedDistanceField::getCollisionSphereGradients | ( | const std::vector< CollisionSphere > & | sphere_list, |
const EigenSTL::vector_Vector3d & | sphere_centers, | ||
GradientInfo & | gradient, | ||
const CollisionType & | type, | ||
double | tolerance, | ||
bool | subtract_radii, | ||
double | maximum_value, | ||
bool | stop_at_first_collision | ||
) |
Definition at line 67 of file collision_distance_field_types.cpp.
double collision_detection::PosedDistanceField::getDistanceGradient | ( | double | x, |
double | y, | ||
double | z, | ||
double & | gradient_x, | ||
double & | gradient_y, | ||
double & | gradient_z, | ||
bool & | in_bounds | ||
) | const [inline] |
Gets not only the distance to the nearest cell but the gradient direction. The point defined by the x, y and z values is transform into the local distance field coordinate system using the current pose. The gradient is computed as a function of the distances of near-by cells. An uninitialized distance is returned if the cell is not valid for gradient production purposes.
x | input x value of the query point |
y | input y value of the query point |
z | input z value of the query point |
gradient_x | output argument with the x value of the gradient |
gradient_y | output argument with the y value of the gradient |
gradient_z | output argument with the z value of the gradient |
in_bounds | true if the point is within the bounds of the distance field, false otherwise |
Reimplemented from distance_field::DistanceField.
Definition at line 149 of file collision_distance_field_types.h.
const Eigen::Affine3d& collision_detection::PosedDistanceField::getPose | ( | ) | const [inline] |
Definition at line 125 of file collision_distance_field_types.h.
void collision_detection::PosedDistanceField::updatePose | ( | const Eigen::Affine3d & | transform | ) | [inline] |
Definition at line 120 of file collision_distance_field_types.h.
Eigen::Affine3d collision_detection::PosedDistanceField::pose_ [protected] |
Definition at line 184 of file collision_distance_field_types.h.