FCL碰撞检测C++测试

1 简介

基本测试，包含碰撞，距离，连续碰撞

2 code demo

#include 
#include 
#include "fcl/narrowphase/distance.h"
#include "fcl/narrowphase/collision.h"
#include "fcl/narrowphase/collision_object.h"
#include "fcl/narrowphase/continuous_collision.h"#include 
#include 
#include 
#include 
#include 
#include 
#include using CollisionGeometryd_sptr = std::shared_ptr<fcl::CollisionGeometry<double>>;void YawToQuaternion(float yaw, Eigen::Quaterniond& q ){q.x() = 0;q.y() = 0;q.z() = sin(yaw/2);q.w() = cos(yaw/2);
}void Pose2dToFclTf( double x, double y, double yaw, fcl::Transform3<double>& tf  ){tf.translation() = fcl::Vector3<double>(x, y, 0);Eigen::Quaterniond q;YawToQuaternion( yaw,q );tf.linear() = q.toRotationMatrix();
}void testBoxVar(CollisionGeometryd_sptr box1, CollisionGeometryd_sptr box2 ){fcl::Transform3<double> box1_tf0;fcl::Transform3<double> box2_tf0;Pose2dToFclTf(0,0,0,box1_tf0);Pose2dToFclTf(0,3,0,box2_tf0);//box1.setTransform(box1_tf0);//box2.setTransform(box2_tf0);fcl::CollisionObject<double> cl_box1(box1, box1_tf0);fcl::CollisionObject<double> cl_box2(box2, box2_tf0);fcl::CollisionRequest<double> request;fcl::CollisionResult<double> result;fcl::DistanceRequest<double> distanceRequest;fcl::DistanceResult<double> distanceResult;distanceRequest.gjk_solver_type = fcl::GJKSolverType::GST_INDEP;distanceRequest.distance_tolerance = 1e-8;fcl::collide(&cl_box1, &cl_box2, request, result);fcl::distance (&cl_box1, &cl_box2, distanceRequest, distanceResult);if (result.isCollision()) {std::cout << "collision !!!" << std::endl;} else {std::cout << "safe !!! d=" << distanceResult.min_distance<< std::endl;}
}void testPosVar( double x1, double y1, double yaw1, double x2, double y2, double yaw2 ){CollisionGeometryd_sptr box1(new  fcl::Box<double>(1.0, 2.0, 0.1)); CollisionGeometryd_sptr box2(new  fcl::Box<double>(1.0, 1.0, 0.1)); fcl::Transform3<double> box1_tf0;fcl::Transform3<double> box2_tf0;Pose2dToFclTf(x1,y1,yaw1,box1_tf0);Pose2dToFclTf(x2,y2,yaw2,box2_tf0);fcl::CollisionObject<double> cl_box1(box1, box1_tf0);fcl::CollisionObject<double> cl_box2(box2, box2_tf0);fcl::CollisionRequest<double> request;fcl::CollisionResult<double> result;fcl::DistanceRequest<double> distanceRequest;fcl::DistanceResult<double> distanceResult;distanceRequest.gjk_solver_type = fcl::GJKSolverType::GST_INDEP;distanceRequest.distance_tolerance = 1e-8;fcl::collide(&cl_box1, &cl_box2, request, result);fcl::distance (&cl_box1, &cl_box2, distanceRequest, distanceResult);if (result.isCollision()) {std::cout << "collision !!!" << std::endl;} else {std::cout << "safe !!! d=" << distanceResult.min_distance<< std::endl;}
}
void testContinuousCollision(){printf("TEST Continuous Collision  !!! \n");CollisionGeometryd_sptr box1(new  fcl::Box<double>(1.0, 2.0, 0.1)); CollisionGeometryd_sptr box2(new  fcl::Box<double>(1.0, 1.0, 0.1)); fcl::Transform3<double> box1_tf0;fcl::Transform3<double> box1_tf1;fcl::Transform3<double> box2_tf0;fcl::Transform3<double> box2_tf1;Pose2dToFclTf(0,0,0,box1_tf0);Pose2dToFclTf(0,3,0,box2_tf0);Pose2dToFclTf(0,3,0,box1_tf1);Pose2dToFclTf(0,0,0,box2_tf1);fcl::CollisionObject<double> cl_box1(box1, box1_tf0);fcl::CollisionObject<double> cl_box2(box2, box2_tf0);fcl::ContinuousCollisionRequest<double>  req;fcl::ContinuousCollisionResult<double>  res;// 检测连续碰撞//fcl::continuousCollide(&capsule,tf1_beg, tf1_end, &box,tf2_beg, tf2_end, r1, r2);fcl::continuousCollide(&cl_box1, box1_tf1, &cl_box2, box2_tf1, req, res);if (res.is_collide) {std::cout << "collision !!!" << std::endl;} else {std::cout << "safe !!!" << std::endl;}
}int main(int argc, char* argv[])
{//test_distance_capsule_box1(fcl::GJKSolverType::GST_INDEP, 1e-8);testBoxVar(CollisionGeometryd_sptr(new fcl::Box<double>(1.0, 2.0, 0.1)),CollisionGeometryd_sptr( new fcl::Box<double>(1.0, 2.0, 0.1)));testBoxVar(CollisionGeometryd_sptr(new fcl::Box<double>(1.0, 3.0, 0.1)),CollisionGeometryd_sptr( new fcl::Box<double>(1.0, 3.0, 0.1)));testBoxVar(CollisionGeometryd_sptr(new fcl::Box<double>(2,   0,   0.1)),CollisionGeometryd_sptr( new fcl::Box<double>(2,     0, 0.1)));testPosVar(0,0,1.57, 0,1.5,0);testPosVar(0,0,0, 0,1.5,0);testPosVar(0,0,0, 0,1.51,0);testContinuousCollision();}

cmake_minimum_required(VERSION 2.8)
project(fcl_test)
add_compile_options(-std=c++11)
find_package(FCL REQUIRED)
find_package(CCD REQUIRED)
include_directories(${FCL_INCLUDE_DIRS})
include_directories(${CCD_INCLUDE_DIRS})add_executable(fcl1 ./fcl.cpp)
target_link_libraries(fcl1 ${FCL_LIBRARIES} ${CCD_LIBRARIES})
``

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！