Untitled
unknown
plain_text
2 months ago
1.5 kB
8
Indexable
#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; }
Editor is loading...
Leave a Comment