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);
        }
      }
    }
  }
}
Example #3
0
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);
        }
      }
    }
  }
}