C++ (Cpp) Body::containsPoint示例

示例#1

文件： static_distance_field.cpp 项目： team-vigir/vigir_moveit_advanced

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);
        }
      }
    }
  }
}

示例#2

文件： find_internal_points.cpp 项目： ksenglee/ros

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);
        }
      }
    }
  }
}