void StompCollisionSpace::getVoxelsInBody(const bodies::Body &body, std::vector<tf::Vector3> &voxels)
{
bodies::BoundingSphere bounding_sphere;
body.computeBoundingSphere(bounding_sphere);
int x,y,z,x_min,x_max,y_min,y_max,z_min,z_max;
double xw,yw,zw;
tf::Vector3 v;
worldToGrid(bounding_sphere.center,bounding_sphere.center.x()-bounding_sphere.radius,bounding_sphere.center.y()-bounding_sphere.radius,
bounding_sphere.center.z()-bounding_sphere.radius, x_min,y_min,z_min);
worldToGrid(bounding_sphere.center,bounding_sphere.center.x()+bounding_sphere.radius,bounding_sphere.center.y()+bounding_sphere.radius,
bounding_sphere.center.z()+bounding_sphere.radius, x_max,y_max,z_max);
for(x = x_min; x <= x_max; ++x)
{
for(y = y_min; y <= y_max; ++y)
{
for(z = z_min; z <= z_max; ++z)
{
gridToWorld(bounding_sphere.center,x,y,z,xw,yw,zw);
v.setX(xw);
v.setY(yw);
v.setZ(zw);
// compute all intersections
int count=0;
std::vector<tf::Vector3> pts;
body.intersectsRay(v, tf::Vector3(0, 0, 1), &pts, count);
// if we have an odd number of intersections, we are inside
if (pts.size() % 2 == 1)
voxels.push_back(v);
}
}
}
// ROS_INFO("number of occupied voxels in bounding sphere: %i", voxels.size());
}
void collision_detection::StaticDistanceField::determineCollisionPoints(
const bodies::Body& body,
double resolution,
EigenSTL::vector_Vector3d& points)
{
bodies::BoundingSphere sphere;
body.computeBoundingSphere(sphere);
double xval_s = std::floor((sphere.center.x() - sphere.radius - resolution) / resolution) * resolution;
double yval_s = std::floor((sphere.center.y() - sphere.radius - resolution) / resolution) * resolution;
double zval_s = std::floor((sphere.center.z() - sphere.radius - resolution) / resolution) * resolution;
double xval_e = sphere.center.x() + sphere.radius + resolution;
double yval_e = sphere.center.y() + sphere.radius + resolution;
double zval_e = sphere.center.z() + sphere.radius + resolution;
Eigen::Vector3d pt;
for(pt.x() = xval_s; pt.x() <= xval_e; pt.x() += resolution) {
for(pt.y() = yval_s; pt.y() <= yval_e; pt.y() += resolution) {
for(pt.z() = zval_s; pt.z() <= zval_e; pt.z() += resolution) {
if(body.containsPoint(pt)) {
points.push_back(pt);
}
}
}
}
}
void distance_field::findInternalPointsConvex(
const bodies::Body& body,
double resolution,
EigenSTL::vector_Vector3d& points)
{
bodies::BoundingSphere sphere;
body.computeBoundingSphere(sphere);
double xval_s = std::floor((sphere.center.x() - sphere.radius - resolution) / resolution) * resolution;
double yval_s = std::floor((sphere.center.y() - sphere.radius - resolution) / resolution) * resolution;
double zval_s = std::floor((sphere.center.z() - sphere.radius - resolution) / resolution) * resolution;
double xval_e = sphere.center.x() + sphere.radius + resolution;
double yval_e = sphere.center.y() + sphere.radius + resolution;
double zval_e = sphere.center.z() + sphere.radius + resolution;
Eigen::Vector3d pt;
for(pt.x() = xval_s; pt.x() <= xval_e; pt.x() += resolution) {
for(pt.y() = yval_s; pt.y() <= yval_e; pt.y() += resolution) {
for(pt.z() = zval_s; pt.z() <= zval_e; pt.z() += resolution) {
if(body.containsPoint(pt)) {
points.push_back(pt);
}
}
}
}
}
