1
2
using CollisionGeometryf = CollisionGeometry<float>;
using CollisionGeometryd = CollisionGeometry<double>;

1
2
/// @brief object type: BVH (mesh, points), basic geometry, octree
enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT};

1
2
3
/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, cylinder, convex, plane, halfspace, triangle), and octree
enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
                GEOM_BOX, GEOM_SPHERE, GEOM_ELLIPSOID, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT};

1
2
using Boxf = Box<float>;
using Boxd = Box<double>;

1
2
3
4
5
/// @brief Constructor
Box(S x, S y, S z);

/// @brief Constructor
Box(const Vector3<S>& side);

1
2
using Spheref = Sphere<float>;
using Sphered = Sphere<double>;

1
2
/// @brief Constructor
Sphere(S radius);

1
2
using Ellipsoidf = Ellipsoid<float>;
using Ellipsoidd = Ellipsoid<double>;

1
2
3
4
5
/// @brief Constructor
Ellipsoid(S a, S b, S c);

/// @brief Constructor
Ellipsoid(const Vector3<S>& radii);

1
2
  /// @brief Constructor
  Capsule(S radius, S lz);

1
Cone(S radius, S lz);

1
2
using Cylinderf = Cylinder<float>;
using Cylinderd = Cylinder<double>;

1
2
/// @brief Constructor
Cylinder(S radius, S lz);

1
2
using Convexf = Convex<float>;
using Convexd = Convex<double>;

1
2
3
4
5
/// @brief Constructor
Convex(const std::shared_ptr<const std::vector<Vector3<S>>>& vertices,
       int num_faces, 
       const std::shared_ptr<const std::vector<int>>& faces,
       bool throw_if_invalid = false);

1
2
using Planef = Plane<float>;
using Planed = Plane<double>;

1
2
3
4
5
/// @brief Constructor
Plane(const Vector3<S>& n, S d);

/// @brief Constructor  
Plane(S a, S b, S c, S d);

1
2
using Halfspacef = Halfspace<float>;
using Halfspaced = Halfspace<double>;

1
2
3
4
5
/// @brief Constructor
Halfspace(const Vector3<S>& n, S d);

/// @brief Constructor
Halfspace(S a, S b, S c, S d);

1
2
template<typename BV>
class BVHModel : public CollisionGeometry<typename BV::S>

1
2
using BVHModelf = BVHModel<OBBf>;
using BVHModeld = BVHModel<OBBd>;

1
2
3
4
5
// 创建网格模型
BVHModel<OBB<S>> mesh;
mesh.beginModel();
mesh.addSubModel(vertices, triangles);
mesh.endModel();

1
2
template<typename S>
class OcTree : public CollisionGeometry<S>

1
2
using OcTreef = OcTree<float>;
using OcTreed = OcTree<double>;

1
2
/// @brief Constructor
OcTree(const std::shared_ptr<const octomap::OcTree>& tree);

1
2
3
4
5
6
7
8
9
// R and T are the rotation matrix and translation vector
Matrix3f R;
Vector3f T;
// code for setting R and T
...
// transform is configured according to R and T
Transform3f pose = Transform3f::Identity();
pose.linear() = R;
pose.translation() = T;

1
CollisionRequest (size_t num_max_contacts_=1, bool enable_contact_=false, size_t num_max_cost_sources_=1, bool enable_cost_=false, bool use_approximate_cost_=true, GJKSolverType gjk_solver_type_=GST_LIBCCD)

1
enum  	GJKSolverType { GST_LIBCCD, GST_INDEP }

1
2
using CollisionResultf = CollisionResult<float>;
using CollisionResultd = CollisionResult<double>;

1
size_t numContacts() const;

1
void getContacts(std::vector<Contact<S>>& contacts) const;

1
bool isCollision() const;

1
void clear();

1
2
3
4
5
6
7
template<typename S>
struct Contact {
  Vector3<S> pos;           // 接触点位置
  Vector3<S> normal;        // 接触点法向量
  S penetration_depth;      // 穿透深度
  int b1, b2;              // 碰撞的两个基本形状索引
};

1
2
3
4
5
6
7
8
ContinuousCollisionRequest(size_t num_max_contacts_ = 1,
                          bool enable_contact_ = false,
                          size_t num_max_cost_sources_ = 1,
                          bool enable_cost_ = false,
                          bool use_approximate_cost_ = true,
                          CCDSolverType ccd_solver_type_ = CCDC_NAIVE,
                          GJKSolverType gjk_solver_type_ = GST_LIBCCD,
                          S ccd_motion_type_ = CCDM_TRANS);

1
2
3
4
5
6
enum CCDSolverType {
  CCDC_NAIVE,                    // 朴素方法
  CCDC_CONSERVATIVE_ADVANCEMENT, // 保守推进方法
  CCDC_RAY_SHOOTING,            // 射线投射方法
  CCDC_POLYNOMIAL_SOLVER        // 多项式求解方法
};

1
2
3
4
bool is_collide;              // 是否发生碰撞
S time_of_contact;           // 碰撞发生时间 (0-1之间)
Vector3<S> contact_tf1;      // 对象1在碰撞时刻的位置
Vector3<S> contact_tf2;      // 对象2在碰撞时刻的位置

1
void clear();

1
2
3
4
5
6
7
8
9
// Given two objects o1 and o2
CollisionObjectf* o1 = ...
CollisionObjectf* o2 = ...
// set the collision request structure, here we just use the default setting
CollisionRequest request;
// result will be returned via the collision result structure
CollisionResult result;
// perform collision test
collide(o1, o2, request, result);

1
2
3
4
5
6
7
8
9
// Given two objects o1 and o2
CollisionObjectf* o1 = ...
CollisionObjectf* o2 = ...
// set the distance request structure, here we just use the default setting
DistanceRequest request;
// result will be returned via the collision result structure
DistanceResult result;
// perform distance test
distance(o1, o2, request, result);

1
2
3
4
5
6
7
8
9
10
11
12
13
// Given two objects o1 and o2
CollisionObjectf* o1 = ...
CollisionObjectf* o2 = ...
// The goal transforms for o1 and o2
Transform3f tf_goal_o1 = ...
Transform3f tf_goal_o2 = ...
// set the continuous collision request structure, here we just use the default
// settin
ContinuousCollisionRequest request;
// result will be returned via the continuous collision result structure
ContinuousCollisionResult result;
// perform continuous collision test
continuousCollide(o1, tf_goal_o1, o2, tf_goal_o2, request, result);

1
2
3
4
5
6
7
// Initialize the collision manager for the first group of objects. 
// FCL provides various different implementations of CollisionManager.
// Generally, the DynamicAABBTreeCollisionManager would provide the best
// performance.
BroadPhaseCollisionManagerf* manager1 = new DynamicAABBTreeCollisionManagerf(); 
// Initialize the collision manager for the second group of objects.
BroadPhaseCollisionManagerf* manager2 = new DynamicAABBTreeCollisionManagerf();

1
2
3
4
5
6
7
8
9
// To add objects into the collision manager, using
// BroadPhaseCollisionManager::registerObject() function to add one object
std::vector<CollisionObjectf*> objects1 = ...
for(std::size_t i = 0; i < objects1.size(); ++i)
manager1->registerObject(objects1[i]);
// Another choose is to use BroadPhaseCollisionManager::registerObjects()
// function to add a set of objects
std::vector<CollisionObjectf*> objects2 = ...
manager2->registerObjects(objects2);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
// In order to collect the information during broadphase, CollisionManager
// requires two settings:
// a) a callback to collision or distance; 
// b) an intermediate data to store the information generated during the
//    broadphase computation.
// For convenience, FCL provides default callbacks to satisfy a) and a
// corresponding call back data to satisfy b) for both collision and distance
// queries. For collision use DefaultCollisionCallback and DefaultCollisionData
// and for distance use DefaultDistanceCallback and DefaultDistanceData.
// The default collision/distance data structs are simply containers which
// include the request and distance structures for each query type as mentioned
// above.
DefaultCollisionData collision_data;
DefaultDistanceData distance_data;
// Setup the managers, which is related with initializing the broadphase
// acceleration structure according to objects input
manager1->setup();
manager2->setup();

// Examples for various queries
// 1. Collision query between two object groups and get collision numbers
manager2->collide(manager1, &collision_data, DefaultCollisionFunction);
int n_contact_num = collision_data.result.numContacts(); 
// 2. Distance query between two object groups and get the minimum distance
manager2->distance(manager1, &distance_data, DefaultDistanceFunction);
double min_distance = distance_data.result.min_distance;
// 3. Self collision query for group 1
manager1->collide(&collision_data, DefaultCollisionFunction);
// 4. Self distance query for group 1
manager1->distance(&distance_data, DefaultDistanceFunction);
// 5. Collision query between one object in group 1 and the entire group 2
manager2->collide(objects1[0], &collision_data, DefaultCollisionFunction);
// 6. Distance query between one object in group 1 and the entire group 2
manager2->distance(objects1[0], &distance_data, DefaultDistanceFunction);