Untitled

#include <fcl/geometry/shape/capsule.h>
#include <fcl/geometry/geometric_shape_to_BVH.h>
#include <fcl/BVH/BVH_model.h 
#include <fcl/distance.h>
#include <fcl/common/default_allocator.h>

int main() {
  // Define the parameters for the first capsule
  fcl::Capsule<double> capsule1(1.0, 2.0); // radius = 1.0, half-length = 2.0
  fcl::Transform3<double> transform1;
  transform1.setIdentity();

  // Define the parameters for the second capsule
  fcl::Capsule<double> capsule2(1.5, 3.0); // radius = 1.5, half-length = 3.0
  fcl::Transform3<double> transform2;
  transform2.setTranslation(fcl::Vector3<double>(0, 0, 5)); // translate capsule2 by (0, 0, 5)
  
    // Create CollisionObjects for the capsules
  fcl::CollisionObject<double> obj1(&capsule1, transform1);
    fcl::CollisionObject<double> obj2(&capsule2, transform2);

  // Define the distance request structure
  fcl::DistanceRequest<double> request;
  request.enable_nearest_points = true;

  // Define the distance result structure
  fcl::DistanceResult<double> result;

  // Calculate the distance between the capsules
  double distance = fcl::distance(&obj1, &obj2, request, result);

  // Output the results
  std::cout << "Distance between capsules: " << distance << std::endl;
  if (request.enable_nearest_points) {
      std::cout << "Nearest point on capsule 1: " << result.nearest_points[0] << std::endl;
      std::cout << "Nearest point on capsule 2: " << result.nearest_points[1] << std::endl;
  }
  return 0;
}