Namespaces | Classes | Typedefs | Enumerations | Functions
hpp::fcl Namespace Reference

Namespaces

 details
 for OBB and RSS, there is local coordinate of BV, so normal need to be transformed
 
 time
 Namespace containing time datatypes and time operations.
 
 tools
 

Classes

class  AABB
 A class describing the AABB collision structure, which is a box in 3D space determined by two diagonal points. More...
 
class  Box
 Center at zero point, axis aligned box. More...
 
class  BVFitter
 The class for the default algorithm fitting a bounding volume to a set of points. More...
 
class  BVFitter< kIOS >
 Specification of BVFitter for kIOS bounding volume. More...
 
class  BVFitter< OBB >
 Specification of BVFitter for OBB bounding volume. More...
 
class  BVFitter< OBBRSS >
 Specification of BVFitter for OBBRSS bounding volume. More...
 
class  BVFitter< RSS >
 Specification of BVFitter for RSS bounding volume. More...
 
class  BVFitterBase
 Interface for fitting a bv given the triangles or points inside it. More...
 
class  BVHCollisionTraversalNode
 Traversal node for collision between BVH models. More...
 
class  BVHDistanceTraversalNode
 Traversal node for distance computation between BVH models. More...
 
struct  BVHFrontNode
 Front list acceleration for collision Front list is a set of internal and leaf nodes in the BVTT hierarchy, where the traversal terminates while performing a query during a given time instance. More...
 
class  BVHModel
 A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh) More...
 
class  BVHShapeCollisionTraversalNode
 Traversal node for collision between BVH and shape. More...
 
class  BVHShapeDistanceTraversalNode
 Traversal node for distance computation between BVH and shape. More...
 
struct  BVNode
 A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and also the geometry data provided in BV template parameter. More...
 
struct  BVNodeBase
 BVNodeBase encodes the tree structure for BVH. More...
 
class  BVSplitter
 A class describing the split rule that splits each BV node. More...
 
class  BVSplitterBase
 Base interface for BV splitting algorithm. More...
 
class  CachedMeshLoader
 Class for building polyhedron from files with cache mechanism. More...
 
class  Capsule
 Center at zero point capsule. More...
 
struct  CollisionFunctionMatrix
 collision matrix stores the functions for collision between different types of objects and provides a uniform call interface More...
 
class  CollisionGeometry
 The geometry for the object for collision or distance computation. More...
 
class  CollisionObject
 the object for collision or distance computation, contains the geometry and the transform information More...
 
struct  CollisionRequest
 request to the collision algorithm More...
 
struct  CollisionResult
 collision result More...
 
class  CollisionTraversalNodeBase
 Node structure encoding the information required for collision traversal. More...
 
class  Cone
 Center at zero cone. More...
 
struct  Contact
 Contact information returned by collision. More...
 
class  Convex
 Convex polytope. More...
 
class  Cylinder
 Center at zero cylinder. More...
 
struct  DistanceFunctionMatrix
 distance matrix stores the functions for distance between different types of objects and provides a uniform call interface More...
 
struct  DistanceRequest
 request to the distance computation More...
 
struct  DistanceResult
 distance result More...
 
class  DistanceTraversalNodeBase
 Node structure encoding the information required for distance traversal. More...
 
struct  GJKSolver_indep
 collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet) More...
 
class  Halfspace
 Half Space: this is equivalent to the Plane in ODE. More...
 
class  Intersect
 CCD intersect kernel among primitives. More...
 
class  KDOP
 KDOP class describes the KDOP collision structures. More...
 
class  kIOS
 A class describing the kIOS collision structure, which is a set of spheres. More...
 
class  MeshCollisionTraversalNode
 Traversal node for collision between two meshes. More...
 
class  MeshDistanceTraversalNode
 Traversal node for distance computation between two meshes. More...
 
class  MeshDistanceTraversalNodekIOS
 
class  MeshDistanceTraversalNodeOBBRSS
 
class  MeshDistanceTraversalNodeRSS
 Traversal node for distance computation between two meshes if their underlying BVH node is oriented node (RSS, OBBRSS, kIOS) More...
 
class  MeshLoader
 Base class for building polyhedron from files. More...
 
class  MeshOcTreeCollisionTraversalNode
 Traversal node for mesh-octree collision. More...
 
class  MeshOcTreeDistanceTraversalNode
 Traversal node for mesh-octree distance. More...
 
class  MeshShapeCollisionTraversalNode
 Traversal node for collision between mesh and shape. More...
 
class  MeshShapeCollisionTraversalNodekIOS
 
class  MeshShapeCollisionTraversalNodeOBB
 Traversal node for mesh and shape, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS) More...
 
class  MeshShapeCollisionTraversalNodeOBBRSS
 
class  MeshShapeCollisionTraversalNodeRSS
 
class  MeshShapeDistanceTraversalNode
 Traversal node for distance between mesh and shape. More...
 
class  MeshShapeDistanceTraversalNodekIOS
 
class  MeshShapeDistanceTraversalNodeOBBRSS
 
class  MeshShapeDistanceTraversalNodeRSS
 Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS, OBBRSS, kIOS) More...
 
class  OBB
 Oriented bounding box class. More...
 
class  OBBRSS
 Class merging the OBB and RSS, can handle collision and distance simultaneously. More...
 
class  OcTree
 Octree is one type of collision geometry which can encode uncertainty information in the sensor data. More...
 
class  OcTreeCollisionTraversalNode
 Traversal node for octree collision. More...
 
class  OcTreeDistanceTraversalNode
 Traversal node for octree distance. More...
 
class  OcTreeMeshCollisionTraversalNode
 Traversal node for octree-mesh collision. More...
 
class  OcTreeMeshDistanceTraversalNode
 Traversal node for octree-mesh distance. More...
 
class  OcTreeShapeCollisionTraversalNode
 Traversal node for octree-shape collision. More...
 
class  OcTreeShapeDistanceTraversalNode
 Traversal node for octree-shape distance. More...
 
class  OcTreeSolver
 Algorithms for collision related with octree. More...
 
class  Plane
 Infinite plane. More...
 
class  PolySolver
 A class solves polynomial degree (1,2,3) equations. More...
 
class  Project
 Project functions. More...
 
class  RSS
 A class for rectangle sphere-swept bounding volume. More...
 
class  ShapeBase
 Base class for all basic geometric shapes. More...
 
class  ShapeBVHCollisionTraversalNode
 Traversal node for collision between shape and BVH. More...
 
class  ShapeBVHDistanceTraversalNode
 Traversal node for distance computation between shape and BVH. More...
 
class  ShapeCollisionTraversalNode
 Traversal node for collision between two shapes. More...
 
class  ShapeDistanceTraversalNode
 Traversal node for distance between two shapes. More...
 
class  ShapeMeshCollisionTraversalNode
 Traversal node for collision between shape and mesh. More...
 
class  ShapeMeshCollisionTraversalNodekIOS
 
class  ShapeMeshCollisionTraversalNodeOBB
 Traversal node for shape and mesh, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS) More...
 
class  ShapeMeshCollisionTraversalNodeOBBRSS
 
class  ShapeMeshCollisionTraversalNodeRSS
 
class  ShapeMeshDistanceTraversalNode
 Traversal node for distance between shape and mesh. More...
 
class  ShapeMeshDistanceTraversalNodekIOS
 
class  ShapeMeshDistanceTraversalNodeOBBRSS
 
class  ShapeMeshDistanceTraversalNodeRSS
 
class  ShapeOcTreeCollisionTraversalNode
 Traversal node for shape-octree collision. More...
 
class  ShapeOcTreeDistanceTraversalNode
 Traversal node for shape-octree distance. More...
 
class  Sphere
 Center at zero point sphere. More...
 
class  Transform3f
 Simple transform class used locally by InterpMotion. More...
 
class  TraversalNodeBase
 Node structure encoding the information required for traversal. More...
 
class  Triangle
 Triangle with 3 indices for points. More...
 
struct  TriangleAndVertices
 
class  TriangleDistance
 Triangle distance functions. More...
 
class  TriangleP
 Triangle stores the points instead of only indices of points. More...
 
class  Variance3f
 Class for variance matrix in 3d. More...
 

Typedefs

typedef std::list< BVHFrontNodeBVHFrontList
 BVH front list is a list of front nodes. More...
 
typedef double FCL_REAL
 
typedef boost::uint64_t FCL_INT64
 
typedef boost::int64_t FCL_UINT64
 
typedef boost::uint32_t FCL_UINT32
 
typedef boost::int32_t FCL_INT32
 
typedef boost::shared_ptr< CollisionObjectCollisionObjectPtr_t
 
typedef boost::shared_ptr< const CollisionObjectCollisionObjectConstPtr_t
 
typedef boost::shared_ptr< CollisionGeometryCollisionGeometryPtr_t
 
typedef boost::shared_ptr< const CollisionGeometryCollisionGeometryConstPtr_t
 
typedef Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
 
typedef Eigen::Quaternion< FCL_REALQuaternion3f
 
typedef Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
 
typedef MeshCollisionTraversalNode< OBB, 0 > MeshCollisionTraversalNodeOBB
 Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS) More...
 
typedef MeshCollisionTraversalNode< RSS, 0 > MeshCollisionTraversalNodeRSS
 
typedef MeshCollisionTraversalNode< kIOS, 0 > MeshCollisionTraversalNodekIOS
 
typedef MeshCollisionTraversalNode< OBBRSS, 0 > MeshCollisionTraversalNodeOBBRSS
 

Enumerations

enum  SplitMethodType {
  SPLIT_METHOD_MEAN,
  SPLIT_METHOD_MEDIAN,
  SPLIT_METHOD_BV_CENTER
}
 Three types of split algorithms are provided in FCL as default. More...
 
enum  BVHBuildState {
  BVH_BUILD_STATE_EMPTY,
  BVH_BUILD_STATE_BEGUN,
  BVH_BUILD_STATE_PROCESSED,
  BVH_BUILD_STATE_UPDATE_BEGUN,
  BVH_BUILD_STATE_UPDATED,
  BVH_BUILD_STATE_REPLACE_BEGUN
}
 States for BVH construction empty->begun->processed ->replace_begun->processed -> ...... More...
 
enum  BVHReturnCode {
  BVH_OK = 0,
  BVH_ERR_MODEL_OUT_OF_MEMORY = -1,
  BVH_ERR_BUILD_OUT_OF_SEQUENCE = -2,
  BVH_ERR_BUILD_EMPTY_MODEL = -3,
  BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME = -4,
  BVH_ERR_UNSUPPORTED_FUNCTION = -5,
  BVH_ERR_UNUPDATED_MODEL = -6,
  BVH_ERR_INCORRECT_DATA = -7,
  BVH_ERR_UNKNOWN = -8
}
 Error code for BVH. More...
 
enum  BVHModelType {
  BVH_MODEL_UNKNOWN,
  BVH_MODEL_TRIANGLES,
  BVH_MODEL_POINTCLOUD
}
 BVH model type. More...
 
enum  GJKSolverType { GST_INDEP }
 Type of narrow phase GJK solver. More...
 
enum  CollisionRequestFlag {
  CONTACT = 0x00001,
  DISTANCE_LOWER_BOUND = 0x00002,
  NO_REQUEST = 0x01000
}
 flag declaration for specifying required params in CollisionResult More...
 
enum  OBJECT_TYPE {
  OT_UNKNOWN,
  OT_BVH,
  OT_GEOM,
  OT_OCTREE,
  OT_COUNT
}
 object type: BVH (mesh, points), basic geometry, octree More...
 
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_CAPSULE,
  GEOM_CONE,
  GEOM_CYLINDER,
  GEOM_CONVEX,
  GEOM_PLANE,
  GEOM_HALFSPACE,
  GEOM_TRIANGLE,
  GEOM_OCTREE,
  NODE_COUNT
}
 traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, capsule, cone, cylinder, convex, plane, triangle), and octree More...
 
enum  { RelativeTransformationIsIdentity = 1 }
 

Functions

static AABB translate (const AABB &aabb, const Vec3f &t)
 translate the center of AABB by t More...
 
static AABB rotate (const AABB &aabb, const Matrix3f &t)
 
bool overlap (const Matrix3f &R0, const Vec3f &T0, const AABB &b1, const AABB &b2)
 Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity. More...
 
bool overlap (const Matrix3f &R0, const Vec3f &T0, const AABB &b1, const AABB &b2, const CollisionRequest &request, FCL_REAL &sqrDistLowerBound)
 Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity. More...
 
template<typename BV1 , typename BV2 >
static void convertBV (const BV1 &bv1, const Transform3f &tf1, BV2 &bv2)
 Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity configuration. More...
 
template<size_t N>
bool overlap (const Matrix3f &, const Vec3f &, const KDOP< N > &, const KDOP< N > &)
 
template<size_t N>
bool overlap (const Matrix3f &, const Vec3f &, const KDOP< N > &, const KDOP< N > &, const CollisionRequest &, FCL_REAL &)
 
template<size_t N>
KDOP< N > translate (const KDOP< N > &bv, const Vec3f &t)
 translate the KDOP BV More...
 
kIOS translate (const kIOS &bv, const Vec3f &t)
 Translate the kIOS BV. More...
 
bool overlap (const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2)
 Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity. More...
 
bool overlap (const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, const CollisionRequest &request, FCL_REAL &sqrDistLowerBound)
 Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity. More...
 
FCL_REAL distance (const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
 Approximate distance between two kIOS bounding volumes. More...
 
OBB translate (const OBB &bv, const Vec3f &t)
 Translate the OBB bv. More...
 
bool overlap (const Matrix3f &R0, const Vec3f &T0, const OBB &b1, const OBB &b2)
 Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity. More...
 
bool overlap (const Matrix3f &R0, const Vec3f &T0, const OBB &b1, const OBB &b2, const CollisionRequest &request, FCL_REAL &sqrDistLowerBound)
 Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity. More...
 
bool obbDisjoint (const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b)
 Check collision between two boxes. More...
 
OBBRSS translate (const OBBRSS &bv, const Vec3f &t)
 Translate the OBBRSS bv. More...
 
bool overlap (const Matrix3f &R0, const Vec3f &T0, const OBBRSS &b1, const OBBRSS &b2)
 Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity. More...
 
bool overlap (const Matrix3f &R0, const Vec3f &T0, const OBBRSS &b1, const OBBRSS &b2, const CollisionRequest &request, FCL_REAL &sqrDistLowerBound)
 Check collision between two OBBRSS. More...
 
FCL_REAL distance (const Matrix3f &R0, const Vec3f &T0, const OBBRSS &b1, const OBBRSS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
 Computate distance between two OBBRSS, b1 is in configuation (R0, T0) and b2 is in indentity; P and Q, is not NULL, returns the nearest points. More...
 
RSS translate (const RSS &bv, const Vec3f &t)
 Translate the RSS bv. More...
 
FCL_REAL distance (const Matrix3f &R0, const Vec3f &T0, const RSS &b1, const RSS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
 distance between two RSS bounding volumes P and Q (optional return values) are the closest points in the rectangles, not the RSS. More...
 
bool overlap (const Matrix3f &R0, const Vec3f &T0, const RSS &b1, const RSS &b2)
 Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity. More...
 
bool overlap (const Matrix3f &R0, const Vec3f &T0, const RSS &b1, const RSS &b2, const CollisionRequest &request, FCL_REAL &sqrDistLowerBound)
 Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity. More...
 
template<typename BV >
void fit (Vec3f *ps, int n, BV &bv)
 Compute a bounding volume that fits a set of n points. More...
 
template<>
void fit< OBB > (Vec3f *ps, int n, OBB &bv)
 
template<>
void fit< RSS > (Vec3f *ps, int n, RSS &bv)
 
template<>
void fit< kIOS > (Vec3f *ps, int n, kIOS &bv)
 
template<>
void fit< OBBRSS > (Vec3f *ps, int n, OBBRSS &bv)
 
void updateFrontList (BVHFrontList *front_list, int b1, int b2)
 Add new front node into the front list. More...
 
template<typename BV >
void BVHExpand (BVHModel< BV > &model, const Variance3f *ucs, FCL_REAL r)
 Expand the BVH bounding boxes according to the variance matrix corresponding to the data stored within each BV node. More...
 
void BVHExpand (BVHModel< OBB > &model, const Variance3f *ucs, FCL_REAL r)
 Expand the BVH bounding boxes according to the corresponding variance information, for OBB. More...
 
void BVHExpand (BVHModel< RSS > &model, const Variance3f *ucs, FCL_REAL r)
 Expand the BVH bounding boxes according to the corresponding variance information, for RSS. More...
 
template<typename BV >
BVHModel< BV > * BVHExtract (const BVHModel< BV > &model, const Transform3f &pose, const AABB &aabb)
 Extract the part of the BVHModel that is inside an AABB. More...
 
void getCovariance (Vec3f *ps, Vec3f *ps2, Triangle *ts, unsigned int *indices, int n, Matrix3f &M)
 Compute the covariance matrix for a set or subset of points. if ts = null, then indices refer to points directly; otherwise refer to triangles. More...
 
void getRadiusAndOriginAndRectangleSize (Vec3f *ps, Vec3f *ps2, Triangle *ts, unsigned int *indices, int n, const Matrix3f &axes, Vec3f &origin, FCL_REAL l[2], FCL_REAL &r)
 Compute the RSS bounding volume parameters: radius, rectangle size and the origin, given the BV axises. More...
 
void getExtentAndCenter (Vec3f *ps, Vec3f *ps2, Triangle *ts, unsigned int *indices, int n, Matrix3f &axes, Vec3f &center, Vec3f &extent)
 Compute the bounding volume extent and center for a set or subset of points, given the BV axises. More...
 
void circumCircleComputation (const Vec3f &a, const Vec3f &b, const Vec3f &c, Vec3f &center, FCL_REAL &radius)
 Compute the center and radius for a triangle's circumcircle. More...
 
FCL_REAL maximumDistance (Vec3f *ps, Vec3f *ps2, Triangle *ts, unsigned int *indices, int n, const Vec3f &query)
 Compute the maximum distance from a given center point to a point cloud. More...
 
std::size_t collide (const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
 Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. More...
 
std::size_t collide (const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const CollisionRequest &request, CollisionResult &result)
 
CollisionRequestFlag operator~ (CollisionRequestFlag a)
 
CollisionRequestFlag operator| (CollisionRequestFlag a, CollisionRequestFlag b)
 
CollisionRequestFlag operator& (CollisionRequestFlag a, CollisionRequestFlag b)
 
CollisionRequestFlag operator^ (CollisionRequestFlag a, CollisionRequestFlag b)
 
CollisionRequestFlagoperator|= (CollisionRequestFlag &a, CollisionRequestFlag b)
 
CollisionRequestFlagoperator&= (CollisionRequestFlag &a, CollisionRequestFlag b)
 
CollisionRequestFlagoperator^= (CollisionRequestFlag &a, CollisionRequestFlag b)
 
void collide (CollisionTraversalNodeBase *node, const CollisionRequest &request, CollisionResult &result, BVHFrontList *front_list=NULL)
 collision on collision traversal node More...
 
void distance (DistanceTraversalNodeBase *node, BVHFrontList *front_list=NULL, int qsize=2)
 distance computation on distance traversal node; can use front list to accelerate More...
 
CollisionGeometryextract (const CollisionGeometry *model, const Transform3f &pose, const AABB &aabb)
 
FCL_REAL distance (const CollisionObject *o1, const CollisionObject *o2, const DistanceRequest &request, DistanceResult &result)
 Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them. More...
 
FCL_REAL distance (const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result)
 
template<typename Derived >
static Derived::Scalar triple (const Eigen::MatrixBase< Derived > &x, const Eigen::MatrixBase< Derived > &y, const Eigen::MatrixBase< Derived > &z)
 
template<typename Derived1 , typename Derived2 , typename Derived3 >
void generateCoordinateSystem (const Eigen::MatrixBase< Derived1 > &_w, const Eigen::MatrixBase< Derived2 > &_u, const Eigen::MatrixBase< Derived3 > &_v)
 
template<typename Derived , typename OtherDerived >
void hat (const Eigen::MatrixBase< Derived > &mat, const Eigen::MatrixBase< OtherDerived > &vec)
 
template<typename Derived , typename OtherDerived >
void relativeTransform (const Eigen::MatrixBase< Derived > &R1, const Eigen::MatrixBase< OtherDerived > &t1, const Eigen::MatrixBase< Derived > &R2, const Eigen::MatrixBase< OtherDerived > &t2, const Eigen::MatrixBase< Derived > &R, const Eigen::MatrixBase< OtherDerived > &t)
 
template<typename Derived , typename Vector >
void eigen (const Eigen::MatrixBase< Derived > &m, typename Derived::Scalar dout[3], Vector *vout)
 compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen vectors More...
 
template<typename Derived , typename OtherDerived >
Derived::Scalar quadraticForm (const Eigen::MatrixBase< Derived > &R, const Eigen::MatrixBase< OtherDerived > &v)
 
template<typename Derived , typename OtherDerived >
bool isEqual (const Eigen::MatrixBase< Derived > &lhs, const Eigen::MatrixBase< OtherDerived > &rhs, const FCL_REAL tol=std::numeric_limits< FCL_REAL >::epsilon()*100)
 
template<typename Derived >
Derived & setEulerZYX (const Eigen::MatrixBase< Derived > &R, FCL_REAL eulerX, FCL_REAL eulerY, FCL_REAL eulerZ)
 
template<typename Derived >
Derived & setEulerYPR (const Eigen::MatrixBase< Derived > &R, FCL_REAL yaw, FCL_REAL pitch, FCL_REAL roll)
 
static std::ostream & operator<< (std::ostream &o, const Quaternion3f &q)
 
bool isQuatIdentity (const Quaternion3f &q)
 
bool areQuatEquals (const Quaternion3f &q1, const Quaternion3f &q2)
 
Transform3f inverse (const Transform3f &tf)
 inverse the transform More...
 
void relativeTransform (const Transform3f &tf1, const Transform3f &tf2, Transform3f &tf)
 compute the relative transform between two transforms: tf2 = tf1 * tf (relative to the local coordinate system in tf1) More...
 
void relativeTransform2 (const Transform3f &tf1, const Transform3f &tf2, Transform3f &tf)
 compute the relative transform between two transforms: tf2 = tf * tf1 (relative to the global coordinate system) More...
 
unsigned buildMesh (const fcl::Vec3f &scale, const aiScene *scene, const aiNode *node, unsigned vertices_offset, TriangleAndVertices &tv)
 Recursive procedure for building a mesh. More...
 
template<class BoundingVolume >
void meshFromAssimpScene (const std::string &name, const fcl::Vec3f &scale, const aiScene *scene, const boost::shared_ptr< BVHModel< BoundingVolume > > &mesh) throw (std::invalid_argument)
 Convert an assimp scene to a mesh. More...
 
template<class BoundingVolume >
void loadPolyhedronFromResource (const std::string &resource_path, const fcl::Vec3f &scale, const boost::shared_ptr< BVHModel< BoundingVolume > > &polyhedron) throw (std::invalid_argument)
 Read a mesh file and convert it to a polyhedral mesh. More...
 
static void computeChildBV (const AABB &root_bv, unsigned int i, AABB &child_bv)
 compute the bounding volume of an octree node's i-th child More...
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Box &shape, const Transform3f &pose)
 Generate BVH model from box. More...
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Sphere &shape, const Transform3f &pose, unsigned int seg, unsigned int ring)
 Generate BVH model from sphere, given the number of segments along longitude and number of rings along latitude. More...
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Sphere &shape, const Transform3f &pose, unsigned int n_faces_for_unit_sphere)
 Generate BVH model from sphere The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. More...
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Cylinder &shape, const Transform3f &pose, unsigned int tot, unsigned int h_num)
 Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis. More...
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Cylinder &shape, const Transform3f &pose, unsigned int tot_for_unit_cylinder)
 Generate BVH model from cylinder Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. More...
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Cone &shape, const Transform3f &pose, unsigned int tot, unsigned int h_num)
 Generate BVH model from cone, given the number of segments along circle and the number of segments along axis. More...
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Cone &shape, const Transform3f &pose, unsigned int tot_for_unit_cone)
 Generate BVH model from cone Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. More...
 
template<typename BV , typename S >
void computeBV (const S &s, const Transform3f &tf, BV &bv)
 calculate a bounding volume for a shape in a specific configuration More...
 
template<>
void computeBV< AABB, Box > (const Box &s, const Transform3f &tf, AABB &bv)
 
template<>
void computeBV< AABB, Sphere > (const Sphere &s, const Transform3f &tf, AABB &bv)
 
template<>
void computeBV< AABB, Capsule > (const Capsule &s, const Transform3f &tf, AABB &bv)
 
template<>
void computeBV< AABB, Cone > (const Cone &s, const Transform3f &tf, AABB &bv)
 
template<>
void computeBV< AABB, Cylinder > (const Cylinder &s, const Transform3f &tf, AABB &bv)
 
template<>
void computeBV< AABB, Convex > (const Convex &s, const Transform3f &tf, AABB &bv)
 
template<>
void computeBV< AABB, TriangleP > (const TriangleP &s, const Transform3f &tf, AABB &bv)
 
template<>
void computeBV< AABB, Halfspace > (const Halfspace &s, const Transform3f &tf, AABB &bv)
 
template<>
void computeBV< AABB, Plane > (const Plane &s, const Transform3f &tf, AABB &bv)
 
template<>
void computeBV< OBB, Box > (const Box &s, const Transform3f &tf, OBB &bv)
 
template<>
void computeBV< OBB, Sphere > (const Sphere &s, const Transform3f &tf, OBB &bv)
 
template<>
void computeBV< OBB, Capsule > (const Capsule &s, const Transform3f &tf, OBB &bv)
 
template<>
void computeBV< OBB, Cone > (const Cone &s, const Transform3f &tf, OBB &bv)
 
template<>
void computeBV< OBB, Cylinder > (const Cylinder &s, const Transform3f &tf, OBB &bv)
 
template<>
void computeBV< OBB, Convex > (const Convex &s, const Transform3f &tf, OBB &bv)
 
template<>
void computeBV< OBB, Halfspace > (const Halfspace &s, const Transform3f &tf, OBB &bv)
 
template<>
void computeBV< RSS, Halfspace > (const Halfspace &s, const Transform3f &tf, RSS &bv)
 
template<>
void computeBV< OBBRSS, Halfspace > (const Halfspace &s, const Transform3f &tf, OBBRSS &bv)
 
template<>
void computeBV< kIOS, Halfspace > (const Halfspace &s, const Transform3f &tf, kIOS &bv)
 
template<>
void computeBV< KDOP< 16 >, Halfspace > (const Halfspace &s, const Transform3f &tf, KDOP< 16 > &bv)
 
template<>
void computeBV< KDOP< 18 >, Halfspace > (const Halfspace &s, const Transform3f &tf, KDOP< 18 > &bv)
 
template<>
void computeBV< KDOP< 24 >, Halfspace > (const Halfspace &s, const Transform3f &tf, KDOP< 24 > &bv)
 
template<>
void computeBV< OBB, Plane > (const Plane &s, const Transform3f &tf, OBB &bv)
 
template<>
void computeBV< RSS, Plane > (const Plane &s, const Transform3f &tf, RSS &bv)
 
template<>
void computeBV< OBBRSS, Plane > (const Plane &s, const Transform3f &tf, OBBRSS &bv)
 
template<>
void computeBV< kIOS, Plane > (const Plane &s, const Transform3f &tf, kIOS &bv)
 
template<>
void computeBV< KDOP< 16 >, Plane > (const Plane &s, const Transform3f &tf, KDOP< 16 > &bv)
 
template<>
void computeBV< KDOP< 18 >, Plane > (const Plane &s, const Transform3f &tf, KDOP< 18 > &bv)
 
template<>
void computeBV< KDOP< 24 >, Plane > (const Plane &s, const Transform3f &tf, KDOP< 24 > &bv)
 
void constructBox (const AABB &bv, Box &box, Transform3f &tf)
 construct a box shape (with a configuration) from a given bounding volume More...
 
void constructBox (const OBB &bv, Box &box, Transform3f &tf)
 
void constructBox (const OBBRSS &bv, Box &box, Transform3f &tf)
 
void constructBox (const kIOS &bv, Box &box, Transform3f &tf)
 
void constructBox (const RSS &bv, Box &box, Transform3f &tf)
 
void constructBox (const KDOP< 16 > &bv, Box &box, Transform3f &tf)
 
void constructBox (const KDOP< 18 > &bv, Box &box, Transform3f &tf)
 
void constructBox (const KDOP< 24 > &bv, Box &box, Transform3f &tf)
 
void constructBox (const AABB &bv, const Transform3f &tf_bv, Box &box, Transform3f &tf)
 
void constructBox (const OBB &bv, const Transform3f &tf_bv, Box &box, Transform3f &tf)
 
void constructBox (const OBBRSS &bv, const Transform3f &tf_bv, Box &box, Transform3f &tf)
 
void constructBox (const kIOS &bv, const Transform3f &tf_bv, Box &box, Transform3f &tf)
 
void constructBox (const RSS &bv, const Transform3f &tf_bv, Box &box, Transform3f &tf)
 
void constructBox (const KDOP< 16 > &bv, const Transform3f &tf_bv, Box &box, Transform3f &tf)
 
void constructBox (const KDOP< 18 > &bv, const Transform3f &tf_bv, Box &box, Transform3f &tf)
 
void constructBox (const KDOP< 24 > &bv, const Transform3f &tf_bv, Box &box, Transform3f &tf)
 
Halfspace transform (const Halfspace &a, const Transform3f &tf)
 
Plane transform (const Plane &a, const Transform3f &tf)
 
template<typename S1 , typename S2 , typename NarrowPhaseSolver >
bool initialize (ShapeCollisionTraversalNode< S1, S2, NarrowPhaseSolver > &node, const S1 &shape1, const Transform3f &tf1, const S2 &shape2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, CollisionResult &result)
 Initialize traversal node for collision between two geometric shapes, given current object transform. More...
 
template<typename BV , typename S , typename NarrowPhaseSolver >
bool initialize (MeshShapeCollisionTraversalNode< BV, S, NarrowPhaseSolver > &node, BVHModel< BV > &model1, Transform3f &tf1, const S &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, CollisionResult &result, bool use_refit=false, bool refit_bottomup=false)
 Initialize traversal node for collision between one mesh and one shape, given current object transform. More...
 
template<typename S , typename BV , typename NarrowPhaseSolver >
bool initialize (ShapeMeshCollisionTraversalNode< S, BV, NarrowPhaseSolver > &node, const S &model1, const Transform3f &tf1, BVHModel< BV > &model2, Transform3f &tf2, const NarrowPhaseSolver *nsolver, CollisionResult &result, bool use_refit=false, bool refit_bottomup=false)
 Initialize traversal node for collision between one mesh and one shape, given current object transform. More...
 
template<typename S , typename NarrowPhaseSolver >
bool initialize (MeshShapeCollisionTraversalNodeOBB< S, NarrowPhaseSolver > &node, const BVHModel< OBB > &model1, const Transform3f &tf1, const S &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, CollisionResult &result)
 Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type. More...
 
template<typename S , typename NarrowPhaseSolver >
bool initialize (MeshShapeCollisionTraversalNodeRSS< S, NarrowPhaseSolver > &node, const BVHModel< RSS > &model1, const Transform3f &tf1, const S &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, CollisionResult &result)
 Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type. More...
 
template<typename S , typename NarrowPhaseSolver >
bool initialize (MeshShapeCollisionTraversalNodekIOS< S, NarrowPhaseSolver > &node, const BVHModel< kIOS > &model1, const Transform3f &tf1, const S &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, CollisionResult &result)
 Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type. More...
 
template<typename S , typename NarrowPhaseSolver >
bool initialize (MeshShapeCollisionTraversalNodeOBBRSS< S, NarrowPhaseSolver > &node, const BVHModel< OBBRSS > &model1, const Transform3f &tf1, const S &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, CollisionResult &result)
 Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type. More...
 
template<typename S , typename NarrowPhaseSolver >
bool initialize (ShapeMeshCollisionTraversalNodeOBB< S, NarrowPhaseSolver > &node, const S &model1, const Transform3f &tf1, const BVHModel< OBB > &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, CollisionResult &result)
 Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type. More...
 
template<typename S , typename NarrowPhaseSolver >
bool initialize (ShapeMeshCollisionTraversalNodeRSS< S, NarrowPhaseSolver > &node, const S &model1, const Transform3f &tf1, const BVHModel< RSS > &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, CollisionResult &result)
 Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type. More...
 
template<typename S , typename NarrowPhaseSolver >
bool initialize (ShapeMeshCollisionTraversalNodekIOS< S, NarrowPhaseSolver > &node, const S &model1, const Transform3f &tf1, const BVHModel< kIOS > &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, CollisionResult &result)
 Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type. More...
 
template<typename S , typename NarrowPhaseSolver >
bool initialize (ShapeMeshCollisionTraversalNodeOBBRSS< S, NarrowPhaseSolver > &node, const S &model1, const Transform3f &tf1, const BVHModel< OBBRSS > &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, CollisionResult &result)
 Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type. More...
 
template<typename BV >
bool initialize (MeshCollisionTraversalNode< BV > &node, BVHModel< BV > &model1, Transform3f &tf1, BVHModel< BV > &model2, Transform3f &tf2, CollisionResult &result, bool use_refit=false, bool refit_bottomup=false)
 Initialize traversal node for collision between two meshes, given the current transforms. More...
 
bool initialize (MeshCollisionTraversalNodeOBB &node, const BVHModel< OBB > &model1, const Transform3f &tf1, const BVHModel< OBB > &model2, const Transform3f &tf2, CollisionResult &result)
 Initialize traversal node for collision between two meshes, specialized for OBB type. More...
 
bool initialize (MeshCollisionTraversalNodeRSS &node, const BVHModel< RSS > &model1, const Transform3f &tf1, const BVHModel< RSS > &model2, const Transform3f &tf2, CollisionResult &result)
 Initialize traversal node for collision between two meshes, specialized for RSS type. More...
 
bool initialize (MeshCollisionTraversalNodeOBBRSS &node, const BVHModel< OBBRSS > &model1, const Transform3f &tf1, const BVHModel< OBBRSS > &model2, const Transform3f &tf2, CollisionResult &result)
 Initialize traversal node for collision between two meshes, specialized for OBBRSS type. More...
 
bool initialize (MeshCollisionTraversalNodekIOS &node, const BVHModel< kIOS > &model1, const Transform3f &tf1, const BVHModel< kIOS > &model2, const Transform3f &tf2, CollisionResult &result)
 Initialize traversal node for collision between two meshes, specialized for kIOS type. More...
 
template<typename S1 , typename S2 , typename NarrowPhaseSolver >
bool initialize (ShapeDistanceTraversalNode< S1, S2, NarrowPhaseSolver > &node, const S1 &shape1, const Transform3f &tf1, const S2 &shape2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
 Initialize traversal node for distance between two geometric shapes. More...
 
template<typename BV >
bool initialize (MeshDistanceTraversalNode< BV > &node, BVHModel< BV > &model1, Transform3f &tf1, BVHModel< BV > &model2, Transform3f &tf2, const DistanceRequest &request, DistanceResult &result, bool use_refit=false, bool refit_bottomup=false)
 Initialize traversal node for distance computation between two meshes, given the current transforms. More...
 
bool initialize (MeshDistanceTraversalNodeRSS &node, const BVHModel< RSS > &model1, const Transform3f &tf1, const BVHModel< RSS > &model2, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result)
 Initialize traversal node for distance computation between two meshes, specialized for RSS type. More...
 
bool initialize (MeshDistanceTraversalNodekIOS &node, const BVHModel< kIOS > &model1, const Transform3f &tf1, const BVHModel< kIOS > &model2, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result)
 Initialize traversal node for distance computation between two meshes, specialized for kIOS type. More...
 
bool initialize (MeshDistanceTraversalNodeOBBRSS &node, const BVHModel< OBBRSS > &model1, const Transform3f &tf1, const BVHModel< OBBRSS > &model2, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result)
 Initialize traversal node for distance computation between two meshes, specialized for OBBRSS type. More...
 
template<typename BV , typename S , typename NarrowPhaseSolver >
bool initialize (MeshShapeDistanceTraversalNode< BV, S, NarrowPhaseSolver > &node, BVHModel< BV > &model1, Transform3f &tf1, const S &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest &request, DistanceResult &result, bool use_refit=false, bool refit_bottomup=false)
 Initialize traversal node for distance computation between one mesh and one shape, given the current transforms. More...
 
template<typename S , typename BV , typename NarrowPhaseSolver >
bool initialize (ShapeMeshDistanceTraversalNode< S, BV, NarrowPhaseSolver > &node, const S &model1, const Transform3f &tf1, BVHModel< BV > &model2, Transform3f &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest &request, DistanceResult &result, bool use_refit=false, bool refit_bottomup=false)
 Initialize traversal node for distance computation between one shape and one mesh, given the current transforms. More...
 
template<typename S , typename NarrowPhaseSolver >
bool initialize (MeshShapeDistanceTraversalNodeRSS< S, NarrowPhaseSolver > &node, const BVHModel< RSS > &model1, const Transform3f &tf1, const S &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
 Initialize traversal node for distance computation between one mesh and one shape, specialized for RSS type. More...
 
template<typename S , typename NarrowPhaseSolver >
bool initialize (MeshShapeDistanceTraversalNodekIOS< S, NarrowPhaseSolver > &node, const BVHModel< kIOS > &model1, const Transform3f &tf1, const S &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
 Initialize traversal node for distance computation between one mesh and one shape, specialized for kIOS type. More...
 
template<typename S , typename NarrowPhaseSolver >
bool initialize (MeshShapeDistanceTraversalNodeOBBRSS< S, NarrowPhaseSolver > &node, const BVHModel< OBBRSS > &model1, const Transform3f &tf1, const S &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
 Initialize traversal node for distance computation between one mesh and one shape, specialized for OBBRSS type. More...
 
template<typename S , typename NarrowPhaseSolver >
bool initialize (ShapeMeshDistanceTraversalNodeRSS< S, NarrowPhaseSolver > &node, const S &model1, const Transform3f &tf1, const BVHModel< RSS > &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
 Initialize traversal node for distance computation between one shape and one mesh, specialized for RSS type. More...
 
template<typename S , typename NarrowPhaseSolver >
bool initialize (ShapeMeshDistanceTraversalNodekIOS< S, NarrowPhaseSolver > &node, const S &model1, const Transform3f &tf1, const BVHModel< kIOS > &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
 Initialize traversal node for distance computation between one shape and one mesh, specialized for kIOS type. More...
 
template<typename S , typename NarrowPhaseSolver >
bool initialize (ShapeMeshDistanceTraversalNodeOBBRSS< S, NarrowPhaseSolver > &node, const S &model1, const Transform3f &tf1, const BVHModel< OBBRSS > &model2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
 Initialize traversal node for distance computation between one shape and one mesh, specialized for OBBRSS type. More...
 
void collisionRecurse (CollisionTraversalNodeBase *node, int b1, int b2, BVHFrontList *front_list, FCL_REAL &sqrDistLowerBound)
 Recurse function for collision. More...
 
void collisionRecurse (MeshCollisionTraversalNodeOBB *node, int b1, int b2, const Matrix3f &R, const Vec3f &T, BVHFrontList *front_list)
 Recurse function for collision, specialized for OBB type. More...
 
void collisionRecurse (MeshCollisionTraversalNodeRSS *node, int b1, int b2, const Matrix3f &R, const Vec3f &T, BVHFrontList *front_list)
 Recurse function for collision, specialized for RSS type. More...
 
void distanceRecurse (DistanceTraversalNodeBase *node, int b1, int b2, BVHFrontList *front_list)
 Recurse function for distance. More...
 
void distanceQueueRecurse (DistanceTraversalNodeBase *node, int b1, int b2, BVHFrontList *front_list, int qsize)
 Recurse function for distance, using queue acceleration. More...
 
void propagateBVHFrontListCollisionRecurse (CollisionTraversalNodeBase *node, const CollisionRequest &request, CollisionResult &result, BVHFrontList *front_list)
 Recurse function for front list propagation. More...
 
bool obbDisjointAndLowerBoundDistance (const Matrix3f &B, const Vec3f &T, const Vec3f &a, const Vec3f &b, const CollisionRequest &request, FCL_REAL &squaredLowerBoundDistance)
 
template<typename T_SH1 , typename T_SH2 , typename NarrowPhaseSolver >
FCL_REAL ShapeShapeDistance (const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
 
template<typename T_SH1 , typename T_SH2 , typename NarrowPhaseSolver >
std::size_t ShapeShapeCollide (const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest &request, CollisionResult &result)
 

Typedef Documentation

BVH front list is a list of front nodes.

typedef boost::shared_ptr<const CollisionGeometry> hpp::fcl::CollisionGeometryConstPtr_t
typedef boost::shared_ptr< const CollisionObject> hpp::fcl::CollisionObjectConstPtr_t
typedef boost::shared_ptr<CollisionObject> hpp::fcl::CollisionObjectPtr_t
typedef boost::int32_t hpp::fcl::FCL_INT32
typedef boost::uint64_t hpp::fcl::FCL_INT64
typedef double hpp::fcl::FCL_REAL
typedef boost::uint32_t hpp::fcl::FCL_UINT32
typedef boost::int64_t hpp::fcl::FCL_UINT64
typedef Eigen::Matrix<FCL_REAL, 3, 3> hpp::fcl::Matrix3f

Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)

typedef Eigen::Quaternion<FCL_REAL> hpp::fcl::Quaternion3f
typedef Eigen::Matrix<FCL_REAL, 3, 1> hpp::fcl::Vec3f

Enumeration Type Documentation

anonymous enum
Enumerator
RelativeTransformationIsIdentity 

States for BVH construction empty->begun->processed ->replace_begun->processed -> ......

| |-> update_begun -> updated -> .....

Enumerator
BVH_BUILD_STATE_EMPTY 
BVH_BUILD_STATE_BEGUN 

empty state, immediately after constructor

BVH_BUILD_STATE_PROCESSED 

after beginModel(), state for adding geometry primitives

BVH_BUILD_STATE_UPDATE_BEGUN 

after tree has been build, ready for cd use

BVH_BUILD_STATE_UPDATED 

after beginUpdateModel(), state for updating geometry primitives

BVH_BUILD_STATE_REPLACE_BEGUN 

after tree has been build for updated geometry, ready for ccd use

after beginReplaceModel(), state for replacing geometry primitives

BVH model type.

Enumerator
BVH_MODEL_UNKNOWN 
BVH_MODEL_TRIANGLES 

unknown model type

BVH_MODEL_POINTCLOUD 

triangle model

point cloud model

Error code for BVH.

Enumerator
BVH_OK 
BVH_ERR_MODEL_OUT_OF_MEMORY 

BVH is valid.

BVH_ERR_BUILD_OUT_OF_SEQUENCE 

Cannot allocate memory for vertices and triangles.

BVH_ERR_BUILD_EMPTY_MODEL 

BVH construction does not follow correct sequence.

BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME 

BVH geometry is not prepared.

BVH_ERR_UNSUPPORTED_FUNCTION 

BVH geometry in previous frame is not prepared.

BVH_ERR_UNUPDATED_MODEL 

BVH funtion is not supported.

BVH_ERR_INCORRECT_DATA 

BVH model update failed.

BVH_ERR_UNKNOWN 

BVH data is not valid.

flag declaration for specifying required params in CollisionResult

Enumerator
CONTACT 
DISTANCE_LOWER_BOUND 
NO_REQUEST 

Type of narrow phase GJK solver.

Enumerator
GST_INDEP 

traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, capsule, cone, cylinder, convex, plane, triangle), and octree

Enumerator
BV_UNKNOWN 
BV_AABB 
BV_OBB 
BV_RSS 
BV_kIOS 
BV_OBBRSS 
BV_KDOP16 
BV_KDOP18 
BV_KDOP24 
GEOM_BOX 
GEOM_SPHERE 
GEOM_CAPSULE 
GEOM_CONE 
GEOM_CYLINDER 
GEOM_CONVEX 
GEOM_PLANE 
GEOM_HALFSPACE 
GEOM_TRIANGLE 
GEOM_OCTREE 
NODE_COUNT 

object type: BVH (mesh, points), basic geometry, octree

Enumerator
OT_UNKNOWN 
OT_BVH 
OT_GEOM 
OT_OCTREE 
OT_COUNT 

Three types of split algorithms are provided in FCL as default.

Enumerator
SPLIT_METHOD_MEAN 
SPLIT_METHOD_MEDIAN 
SPLIT_METHOD_BV_CENTER 

Function Documentation

bool hpp::fcl::areQuatEquals ( const Quaternion3f q1,
const Quaternion3f q2 
)
inline
unsigned hpp::fcl::buildMesh ( const fcl::Vec3f scale,
const aiScene *  scene,
const aiNode *  node,
unsigned  vertices_offset,
TriangleAndVertices tv 
)

Recursive procedure for building a mesh.

Parameters
[in]scaleScale to apply when reading the ressource
[in]scenePointer to the assimp scene
[in]nodeCurrent node of the scene
[in]vertices_offsetCurrent number of vertices in the model
tvTriangles and Vertices of the mesh submodels

Referenced by meshFromAssimpScene().

template<typename BV >
void hpp::fcl::BVHExpand ( BVHModel< BV > &  model,
const Variance3f ucs,
FCL_REAL  r 
)
void hpp::fcl::BVHExpand ( BVHModel< OBB > &  model,
const Variance3f ucs,
FCL_REAL  r 
)

Expand the BVH bounding boxes according to the corresponding variance information, for OBB.

void hpp::fcl::BVHExpand ( BVHModel< RSS > &  model,
const Variance3f ucs,
FCL_REAL  r 
)

Expand the BVH bounding boxes according to the corresponding variance information, for RSS.

template<typename BV >
BVHModel<BV>* hpp::fcl::BVHExtract ( const BVHModel< BV > &  model,
const Transform3f pose,
const AABB aabb 
)

Extract the part of the BVHModel that is inside an AABB.

A triangle in collision with the AABB is considered inside.

Referenced by BVHExpand().

void hpp::fcl::circumCircleComputation ( const Vec3f a,
const Vec3f b,
const Vec3f c,
Vec3f center,
FCL_REAL radius 
)

Compute the center and radius for a triangle's circumcircle.

Referenced by BVHExpand().

std::size_t hpp::fcl::collide ( const CollisionObject o1,
const CollisionObject o2,
const CollisionRequest request,
CollisionResult result 
)

Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them.

Return value is the number of contacts generated between the two objects.

std::size_t hpp::fcl::collide ( const CollisionGeometry o1,
const Transform3f tf1,
const CollisionGeometry o2,
const Transform3f tf2,
const CollisionRequest request,
CollisionResult result 
)
void hpp::fcl::collide ( CollisionTraversalNodeBase node,
const CollisionRequest request,
CollisionResult result,
BVHFrontList front_list = NULL 
)

collision on collision traversal node

Parameters
nodenode containing both objects to test,
Return values
squaredlower bound to the distance between the objects if they do not collide.
Parameters
front_listlist of nodes visited by the query, can be used to accelerate computation
void hpp::fcl::collisionRecurse ( CollisionTraversalNodeBase node,
int  b1,
int  b2,
BVHFrontList front_list,
FCL_REAL sqrDistLowerBound 
)

Recurse function for collision.

Parameters
nodecollision node,
b1,b2ids of bounding volume nodes for object 1 and object 2
Return values
sqrDistLowerBoundsquared lower bound on distance between objects.
void hpp::fcl::collisionRecurse ( MeshCollisionTraversalNodeOBB node,
int  b1,
int  b2,
const Matrix3f R,
const Vec3f T,
BVHFrontList front_list 
)

Recurse function for collision, specialized for OBB type.

void hpp::fcl::collisionRecurse ( MeshCollisionTraversalNodeRSS node,
int  b1,
int  b2,
const Matrix3f R,
const Vec3f T,
BVHFrontList front_list 
)

Recurse function for collision, specialized for RSS type.

template<typename BV , typename S >
void hpp::fcl::computeBV ( const S &  s,
const Transform3f tf,
BV &  bv 
)
template<>
void hpp::fcl::computeBV< AABB, Box > ( const Box s,
const Transform3f tf,
AABB bv 
)
template<>
void hpp::fcl::computeBV< AABB, Capsule > ( const Capsule s,
const Transform3f tf,
AABB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< AABB, Cone > ( const Cone s,
const Transform3f tf,
AABB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< AABB, Convex > ( const Convex s,
const Transform3f tf,
AABB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< AABB, Cylinder > ( const Cylinder s,
const Transform3f tf,
AABB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< AABB, Halfspace > ( const Halfspace s,
const Transform3f tf,
AABB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< AABB, Plane > ( const Plane s,
const Transform3f tf,
AABB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< AABB, Sphere > ( const Sphere s,
const Transform3f tf,
AABB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< AABB, TriangleP > ( const TriangleP s,
const Transform3f tf,
AABB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< KDOP< 16 >, Halfspace > ( const Halfspace s,
const Transform3f tf,
KDOP< 16 > &  bv 
)
template<>
void hpp::fcl::computeBV< KDOP< 16 >, Plane > ( const Plane s,
const Transform3f tf,
KDOP< 16 > &  bv 
)
template<>
void hpp::fcl::computeBV< KDOP< 18 >, Halfspace > ( const Halfspace s,
const Transform3f tf,
KDOP< 18 > &  bv 
)
template<>
void hpp::fcl::computeBV< KDOP< 18 >, Plane > ( const Plane s,
const Transform3f tf,
KDOP< 18 > &  bv 
)
template<>
void hpp::fcl::computeBV< KDOP< 24 >, Halfspace > ( const Halfspace s,
const Transform3f tf,
KDOP< 24 > &  bv 
)
template<>
void hpp::fcl::computeBV< KDOP< 24 >, Plane > ( const Plane s,
const Transform3f tf,
KDOP< 24 > &  bv 
)
template<>
void hpp::fcl::computeBV< kIOS, Halfspace > ( const Halfspace s,
const Transform3f tf,
kIOS bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< kIOS, Plane > ( const Plane s,
const Transform3f tf,
kIOS bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< OBB, Box > ( const Box s,
const Transform3f tf,
OBB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< OBB, Capsule > ( const Capsule s,
const Transform3f tf,
OBB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< OBB, Cone > ( const Cone s,
const Transform3f tf,
OBB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< OBB, Convex > ( const Convex s,
const Transform3f tf,
OBB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< OBB, Cylinder > ( const Cylinder s,
const Transform3f tf,
OBB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< OBB, Halfspace > ( const Halfspace s,
const Transform3f tf,
OBB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< OBB, Plane > ( const Plane s,
const Transform3f tf,
OBB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< OBB, Sphere > ( const Sphere s,
const Transform3f tf,
OBB bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< OBBRSS, Halfspace > ( const Halfspace s,
const Transform3f tf,
OBBRSS bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< OBBRSS, Plane > ( const Plane s,
const Transform3f tf,
OBBRSS bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< RSS, Halfspace > ( const Halfspace s,
const Transform3f tf,
RSS bv 
)

Referenced by computeBV().

template<>
void hpp::fcl::computeBV< RSS, Plane > ( const Plane s,
const Transform3f tf,
RSS bv 
)

Referenced by computeBV().

static void hpp::fcl::computeChildBV ( const AABB root_bv,
unsigned int  i,
AABB child_bv 
)
inlinestatic

compute the bounding volume of an octree node's i-th child

References hpp::fcl::AABB::max_, and hpp::fcl::AABB::min_.

Referenced by hpp::fcl::OcTreeSolver< NarrowPhaseSolver >::ShapeOcTreeDistance().

void hpp::fcl::constructBox ( const AABB bv,
Box box,
Transform3f tf 
)

construct a box shape (with a configuration) from a given bounding volume

Referenced by computeBV(), and hpp::fcl::OcTreeSolver< NarrowPhaseSolver >::ShapeOcTreeDistance().

void hpp::fcl::constructBox ( const OBB bv,
Box box,
Transform3f tf 
)
void hpp::fcl::constructBox ( const OBBRSS bv,
Box box,
Transform3f tf 
)
void hpp::fcl::constructBox ( const kIOS bv,
Box box,
Transform3f tf 
)
void hpp::fcl::constructBox ( const RSS bv,
Box box,
Transform3f tf 
)
void hpp::fcl::constructBox ( const KDOP< 16 > &  bv,
Box box,
Transform3f tf 
)
void hpp::fcl::constructBox ( const KDOP< 18 > &  bv,
Box box,
Transform3f tf 
)
void hpp::fcl::constructBox ( const KDOP< 24 > &  bv,
Box box,
Transform3f tf 
)
void hpp::fcl::constructBox ( const AABB bv,
const Transform3f tf_bv,
Box box,
Transform3f tf 
)
void hpp::fcl::constructBox ( const OBB bv,
const Transform3f tf_bv,
Box box,
Transform3f tf 
)
void hpp::fcl::constructBox ( const OBBRSS bv,
const Transform3f tf_bv,
Box box,
Transform3f tf 
)
void hpp::fcl::constructBox ( const kIOS bv,
const Transform3f tf_bv,
Box box,
Transform3f tf 
)
void hpp::fcl::constructBox ( const RSS bv,
const Transform3f tf_bv,
Box box,
Transform3f tf 
)
void hpp::fcl::constructBox ( const KDOP< 16 > &  bv,
const Transform3f tf_bv,
Box box,
Transform3f tf 
)
void hpp::fcl::constructBox ( const KDOP< 18 > &  bv,
const Transform3f tf_bv,
Box box,
Transform3f tf 
)
void hpp::fcl::constructBox ( const KDOP< 24 > &  bv,
const Transform3f tf_bv,
Box box,
Transform3f tf 
)
template<typename BV1 , typename BV2 >
static void hpp::fcl::convertBV ( const BV1 &  bv1,
const Transform3f tf1,
BV2 &  bv2 
)
inlinestatic

Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity configuration.

Referenced by hpp::fcl::OcTreeSolver< NarrowPhaseSolver >::OcTreeShapeIntersect(), hpp::fcl::OcTreeSolver< NarrowPhaseSolver >::ShapeOcTreeDistance(), and hpp::fcl::OcTreeSolver< NarrowPhaseSolver >::ShapeOcTreeIntersect().

FCL_REAL hpp::fcl::distance ( const CollisionObject o1,
const CollisionObject o2,
const DistanceRequest request,
DistanceResult result 
)

Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them.

Return value is the minimum distance generated between the two objects.

FCL_REAL hpp::fcl::distance ( const CollisionGeometry o1,
const Transform3f tf1,
const CollisionGeometry o2,
const Transform3f tf2,
const DistanceRequest request,
DistanceResult result 
)
void hpp::fcl::distance ( DistanceTraversalNodeBase node,
BVHFrontList front_list = NULL,
int  qsize = 2 
)

distance computation on distance traversal node; can use front list to accelerate

FCL_REAL hpp::fcl::distance ( const Matrix3f R0,
const Vec3f T0,
const RSS b1,
const RSS b2,
Vec3f P = NULL,
Vec3f Q = NULL 
)

distance between two RSS bounding volumes P and Q (optional return values) are the closest points in the rectangles, not the RSS.

But the direction P - Q is the correct direction for cloest points Notice that P and Q are both in the local frame of the first RSS (not global frame and not even the local frame of object 1)

FCL_REAL hpp::fcl::distance ( const Matrix3f R0,
const Vec3f T0,
const kIOS b1,
const kIOS b2,
Vec3f P = NULL,
Vec3f Q = NULL 
)
FCL_REAL hpp::fcl::distance ( const Matrix3f R0,
const Vec3f T0,
const OBBRSS b1,
const OBBRSS b2,
Vec3f P = NULL,
Vec3f Q = NULL 
)

Computate distance between two OBBRSS, b1 is in configuation (R0, T0) and b2 is in indentity; P and Q, is not NULL, returns the nearest points.

void hpp::fcl::distanceQueueRecurse ( DistanceTraversalNodeBase node,
int  b1,
int  b2,
BVHFrontList front_list,
int  qsize 
)

Recurse function for distance, using queue acceleration.

void hpp::fcl::distanceRecurse ( DistanceTraversalNodeBase node,
int  b1,
int  b2,
BVHFrontList front_list 
)

Recurse function for distance.

template<typename Derived , typename Vector >
void hpp::fcl::eigen ( const Eigen::MatrixBase< Derived > &  m,
typename Derived::Scalar  dout[3],
Vector *  vout 
)

compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen vectors

Referenced by hpp::fcl::Variance3f::init().

CollisionGeometry* hpp::fcl::extract ( const CollisionGeometry model,
const Transform3f pose,
const AABB aabb 
)
template<typename BV >
void hpp::fcl::fit ( Vec3f ps,
int  n,
BV &  bv 
)

Compute a bounding volume that fits a set of n points.

References fit< kIOS >(), fit< OBB >(), fit< OBBRSS >(), and fit< RSS >().

Referenced by computeBV().

template<>
void hpp::fcl::fit< kIOS > ( Vec3f ps,
int  n,
kIOS bv 
)

Referenced by fit().

template<>
void hpp::fcl::fit< OBB > ( Vec3f ps,
int  n,
OBB bv 
)

Referenced by fit().

template<>
void hpp::fcl::fit< OBBRSS > ( Vec3f ps,
int  n,
OBBRSS bv 
)

Referenced by fit().

template<>
void hpp::fcl::fit< RSS > ( Vec3f ps,
int  n,
RSS bv 
)

Referenced by fit().

template<typename BV >
void hpp::fcl::generateBVHModel ( BVHModel< BV > &  model,
const Box shape,
const Transform3f pose 
)
template<typename BV >
void hpp::fcl::generateBVHModel ( BVHModel< BV > &  model,
const Sphere shape,
const Transform3f pose,
unsigned int  seg,
unsigned int  ring 
)
template<typename BV >
void hpp::fcl::generateBVHModel ( BVHModel< BV > &  model,
const Sphere shape,
const Transform3f pose,
unsigned int  n_faces_for_unit_sphere 
)

Generate BVH model from sphere The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius.

For sphere of radius r, then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s

References generateBVHModel(), and hpp::fcl::Sphere::radius.

template<typename BV >
void hpp::fcl::generateBVHModel ( BVHModel< BV > &  model,
const Cylinder shape,
const Transform3f pose,
unsigned int  tot,
unsigned int  h_num 
)
template<typename BV >
void hpp::fcl::generateBVHModel ( BVHModel< BV > &  model,
const Cylinder shape,
const Transform3f pose,
unsigned int  tot_for_unit_cylinder 
)

Generate BVH model from cylinder Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius.

For cylinder with larger radius, the number of circle split number is r * tot.

References generateBVHModel(), hpp::fcl::Cylinder::lz, and hpp::fcl::Cylinder::radius.

template<typename BV >
void hpp::fcl::generateBVHModel ( BVHModel< BV > &  model,
const Cone shape,
const Transform3f pose,
unsigned int  tot,
unsigned int  h_num 
)
template<typename BV >
void hpp::fcl::generateBVHModel ( BVHModel< BV > &  model,
const Cone shape,
const Transform3f pose,
unsigned int  tot_for_unit_cone 
)

Generate BVH model from cone Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius.

For cone with larger radius, the number of circle split number is r * tot.

References generateBVHModel(), hpp::fcl::Cone::lz, and hpp::fcl::Cone::radius.

template<typename Derived1 , typename Derived2 , typename Derived3 >
void hpp::fcl::generateCoordinateSystem ( const Eigen::MatrixBase< Derived1 > &  _w,
const Eigen::MatrixBase< Derived2 > &  _u,
const Eigen::MatrixBase< Derived3 > &  _v 
)
void hpp::fcl::getCovariance ( Vec3f ps,
Vec3f ps2,
Triangle ts,
unsigned int *  indices,
int  n,
Matrix3f M 
)

Compute the covariance matrix for a set or subset of points. if ts = null, then indices refer to points directly; otherwise refer to triangles.

Referenced by BVHExpand().

void hpp::fcl::getExtentAndCenter ( Vec3f ps,
Vec3f ps2,
Triangle ts,
unsigned int *  indices,
int  n,
Matrix3f axes,
Vec3f center,
Vec3f extent 
)

Compute the bounding volume extent and center for a set or subset of points, given the BV axises.

Referenced by BVHExpand().

void hpp::fcl::getRadiusAndOriginAndRectangleSize ( Vec3f ps,
Vec3f ps2,
Triangle ts,
unsigned int *  indices,
int  n,
const Matrix3f axes,
Vec3f origin,
FCL_REAL  l[2],
FCL_REAL r 
)

Compute the RSS bounding volume parameters: radius, rectangle size and the origin, given the BV axises.

Referenced by BVHExpand().

template<typename Derived , typename OtherDerived >
void hpp::fcl::hat ( const Eigen::MatrixBase< Derived > &  mat,
const Eigen::MatrixBase< OtherDerived > &  vec 
)
template<typename S1 , typename S2 , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( ShapeCollisionTraversalNode< S1, S2, NarrowPhaseSolver > &  node,
const S1 &  shape1,
const Transform3f tf1,
const S2 &  shape2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
CollisionResult result 
)
template<typename BV , typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( MeshShapeCollisionTraversalNode< BV, S, NarrowPhaseSolver > &  node,
BVHModel< BV > &  model1,
Transform3f tf1,
const S &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
CollisionResult result,
bool  use_refit = false,
bool  refit_bottomup = false 
)
template<typename S , typename BV , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( ShapeMeshCollisionTraversalNode< S, BV, NarrowPhaseSolver > &  node,
const S &  model1,
const Transform3f tf1,
BVHModel< BV > &  model2,
Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
CollisionResult result,
bool  use_refit = false,
bool  refit_bottomup = false 
)
template<typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( MeshShapeCollisionTraversalNodeOBB< S, NarrowPhaseSolver > &  node,
const BVHModel< OBB > &  model1,
const Transform3f tf1,
const S &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
CollisionResult result 
)

Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type.

template<typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( MeshShapeCollisionTraversalNodeRSS< S, NarrowPhaseSolver > &  node,
const BVHModel< RSS > &  model1,
const Transform3f tf1,
const S &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
CollisionResult result 
)

Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type.

template<typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( MeshShapeCollisionTraversalNodekIOS< S, NarrowPhaseSolver > &  node,
const BVHModel< kIOS > &  model1,
const Transform3f tf1,
const S &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
CollisionResult result 
)

Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type.

template<typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( MeshShapeCollisionTraversalNodeOBBRSS< S, NarrowPhaseSolver > &  node,
const BVHModel< OBBRSS > &  model1,
const Transform3f tf1,
const S &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
CollisionResult result 
)

Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type.

References BVH_MODEL_TRIANGLES, computeBV(), hpp::fcl::BVHModel< BV >::getModelType(), hpp::fcl::BVHModel< BV >::tri_indices, and hpp::fcl::BVHModel< BV >::vertices.

template<typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( ShapeMeshCollisionTraversalNodeOBB< S, NarrowPhaseSolver > &  node,
const S &  model1,
const Transform3f tf1,
const BVHModel< OBB > &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
CollisionResult result 
)

Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type.

template<typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( ShapeMeshCollisionTraversalNodeRSS< S, NarrowPhaseSolver > &  node,
const S &  model1,
const Transform3f tf1,
const BVHModel< RSS > &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
CollisionResult result 
)

Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type.

template<typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( ShapeMeshCollisionTraversalNodekIOS< S, NarrowPhaseSolver > &  node,
const S &  model1,
const Transform3f tf1,
const BVHModel< kIOS > &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
CollisionResult result 
)

Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type.

template<typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( ShapeMeshCollisionTraversalNodeOBBRSS< S, NarrowPhaseSolver > &  node,
const S &  model1,
const Transform3f tf1,
const BVHModel< OBBRSS > &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
CollisionResult result 
)

Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type.

template<typename BV >
bool hpp::fcl::initialize ( MeshCollisionTraversalNode< BV > &  node,
BVHModel< BV > &  model1,
Transform3f tf1,
BVHModel< BV > &  model2,
Transform3f tf2,
CollisionResult result,
bool  use_refit = false,
bool  refit_bottomup = false 
)
bool hpp::fcl::initialize ( MeshCollisionTraversalNodeOBB node,
const BVHModel< OBB > &  model1,
const Transform3f tf1,
const BVHModel< OBB > &  model2,
const Transform3f tf2,
CollisionResult result 
)

Initialize traversal node for collision between two meshes, specialized for OBB type.

bool hpp::fcl::initialize ( MeshCollisionTraversalNodeRSS node,
const BVHModel< RSS > &  model1,
const Transform3f tf1,
const BVHModel< RSS > &  model2,
const Transform3f tf2,
CollisionResult result 
)

Initialize traversal node for collision between two meshes, specialized for RSS type.

bool hpp::fcl::initialize ( MeshCollisionTraversalNodeOBBRSS node,
const BVHModel< OBBRSS > &  model1,
const Transform3f tf1,
const BVHModel< OBBRSS > &  model2,
const Transform3f tf2,
CollisionResult result 
)

Initialize traversal node for collision between two meshes, specialized for OBBRSS type.

bool hpp::fcl::initialize ( MeshCollisionTraversalNodekIOS node,
const BVHModel< kIOS > &  model1,
const Transform3f tf1,
const BVHModel< kIOS > &  model2,
const Transform3f tf2,
CollisionResult result 
)

Initialize traversal node for collision between two meshes, specialized for kIOS type.

template<typename S1 , typename S2 , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( ShapeDistanceTraversalNode< S1, S2, NarrowPhaseSolver > &  node,
const S1 &  shape1,
const Transform3f tf1,
const S2 &  shape2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
const DistanceRequest request,
DistanceResult result 
)
template<typename BV >
bool hpp::fcl::initialize ( MeshDistanceTraversalNode< BV > &  node,
BVHModel< BV > &  model1,
Transform3f tf1,
BVHModel< BV > &  model2,
Transform3f tf2,
const DistanceRequest request,
DistanceResult result,
bool  use_refit = false,
bool  refit_bottomup = false 
)
bool hpp::fcl::initialize ( MeshDistanceTraversalNodeRSS node,
const BVHModel< RSS > &  model1,
const Transform3f tf1,
const BVHModel< RSS > &  model2,
const Transform3f tf2,
const DistanceRequest request,
DistanceResult result 
)

Initialize traversal node for distance computation between two meshes, specialized for RSS type.

bool hpp::fcl::initialize ( MeshDistanceTraversalNodekIOS node,
const BVHModel< kIOS > &  model1,
const Transform3f tf1,
const BVHModel< kIOS > &  model2,
const Transform3f tf2,
const DistanceRequest request,
DistanceResult result 
)

Initialize traversal node for distance computation between two meshes, specialized for kIOS type.

bool hpp::fcl::initialize ( MeshDistanceTraversalNodeOBBRSS node,
const BVHModel< OBBRSS > &  model1,
const Transform3f tf1,
const BVHModel< OBBRSS > &  model2,
const Transform3f tf2,
const DistanceRequest request,
DistanceResult result 
)

Initialize traversal node for distance computation between two meshes, specialized for OBBRSS type.

template<typename BV , typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( MeshShapeDistanceTraversalNode< BV, S, NarrowPhaseSolver > &  node,
BVHModel< BV > &  model1,
Transform3f tf1,
const S &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
const DistanceRequest request,
DistanceResult result,
bool  use_refit = false,
bool  refit_bottomup = false 
)
template<typename S , typename BV , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( ShapeMeshDistanceTraversalNode< S, BV, NarrowPhaseSolver > &  node,
const S &  model1,
const Transform3f tf1,
BVHModel< BV > &  model2,
Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
const DistanceRequest request,
DistanceResult result,
bool  use_refit = false,
bool  refit_bottomup = false 
)
template<typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( MeshShapeDistanceTraversalNodeRSS< S, NarrowPhaseSolver > &  node,
const BVHModel< RSS > &  model1,
const Transform3f tf1,
const S &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
const DistanceRequest request,
DistanceResult result 
)

Initialize traversal node for distance computation between one mesh and one shape, specialized for RSS type.

template<typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( MeshShapeDistanceTraversalNodekIOS< S, NarrowPhaseSolver > &  node,
const BVHModel< kIOS > &  model1,
const Transform3f tf1,
const S &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
const DistanceRequest request,
DistanceResult result 
)

Initialize traversal node for distance computation between one mesh and one shape, specialized for kIOS type.

template<typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( MeshShapeDistanceTraversalNodeOBBRSS< S, NarrowPhaseSolver > &  node,
const BVHModel< OBBRSS > &  model1,
const Transform3f tf1,
const S &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
const DistanceRequest request,
DistanceResult result 
)

Initialize traversal node for distance computation between one mesh and one shape, specialized for OBBRSS type.

template<typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( ShapeMeshDistanceTraversalNodeRSS< S, NarrowPhaseSolver > &  node,
const S &  model1,
const Transform3f tf1,
const BVHModel< RSS > &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
const DistanceRequest request,
DistanceResult result 
)

Initialize traversal node for distance computation between one shape and one mesh, specialized for RSS type.

References hpp::fcl::details::setupShapeMeshDistanceOrientedNode().

template<typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( ShapeMeshDistanceTraversalNodekIOS< S, NarrowPhaseSolver > &  node,
const S &  model1,
const Transform3f tf1,
const BVHModel< kIOS > &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
const DistanceRequest request,
DistanceResult result 
)

Initialize traversal node for distance computation between one shape and one mesh, specialized for kIOS type.

References hpp::fcl::details::setupShapeMeshDistanceOrientedNode().

template<typename S , typename NarrowPhaseSolver >
bool hpp::fcl::initialize ( ShapeMeshDistanceTraversalNodeOBBRSS< S, NarrowPhaseSolver > &  node,
const S &  model1,
const Transform3f tf1,
const BVHModel< OBBRSS > &  model2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
const DistanceRequest request,
DistanceResult result 
)

Initialize traversal node for distance computation between one shape and one mesh, specialized for OBBRSS type.

References hpp::fcl::details::setupShapeMeshDistanceOrientedNode().

Transform3f hpp::fcl::inverse ( const Transform3f tf)

inverse the transform

template<typename Derived , typename OtherDerived >
bool hpp::fcl::isEqual ( const Eigen::MatrixBase< Derived > &  lhs,
const Eigen::MatrixBase< OtherDerived > &  rhs,
const FCL_REAL  tol = std::numeric_limits<FCL_REAL>::epsilon()*100 
)

Referenced by hpp::fcl::AABB::equal().

bool hpp::fcl::isQuatIdentity ( const Quaternion3f q)
inline
template<class BoundingVolume >
void hpp::fcl::loadPolyhedronFromResource ( const std::string &  resource_path,
const fcl::Vec3f scale,
const boost::shared_ptr< BVHModel< BoundingVolume > > &  polyhedron 
)
throw (std::invalid_argument
)
inline

Read a mesh file and convert it to a polyhedral mesh.

Parameters
[in]resource_pathPath to the ressource mesh file to be read
[in]scaleScale to apply when reading the ressource
[out]polyhedronThe resulted polyhedron

References meshFromAssimpScene().

FCL_REAL hpp::fcl::maximumDistance ( Vec3f ps,
Vec3f ps2,
Triangle ts,
unsigned int *  indices,
int  n,
const Vec3f query 
)

Compute the maximum distance from a given center point to a point cloud.

Referenced by BVHExpand().

template<class BoundingVolume >
void hpp::fcl::meshFromAssimpScene ( const std::string &  name,
const fcl::Vec3f scale,
const aiScene *  scene,
const boost::shared_ptr< BVHModel< BoundingVolume > > &  mesh 
)
throw (std::invalid_argument
)
inline

Convert an assimp scene to a mesh.

Parameters
[in]nameFile (ressource) transformed into an assimp scene in loa
[in]scaleScale to apply when reading the ressource
[in]scenePointer to the assimp scene
[out]meshThe mesh that must be built

References buildMesh(), BVH_OK, hpp::fcl::TriangleAndVertices::triangles_, and hpp::fcl::TriangleAndVertices::vertices_.

Referenced by loadPolyhedronFromResource().

bool hpp::fcl::obbDisjoint ( const Matrix3f B,
const Vec3f T,
const Vec3f a,
const Vec3f b 
)

Check collision between two boxes.

Parameters
B,Torientation and position of first box,
ahalf dimensions of first box,
bhalf dimensions of second box. The second box is in identity configuration.

Referenced by hpp::fcl::OBB::center().

bool hpp::fcl::obbDisjointAndLowerBoundDistance ( const Matrix3f B,
const Vec3f T,
const Vec3f a,
const Vec3f b,
const CollisionRequest request,
FCL_REAL squaredLowerBoundDistance 
)
CollisionRequestFlag hpp::fcl::operator& ( CollisionRequestFlag  a,
CollisionRequestFlag  b 
)
inline
CollisionRequestFlag& hpp::fcl::operator&= ( CollisionRequestFlag a,
CollisionRequestFlag  b 
)
inline
static std::ostream& hpp::fcl::operator<< ( std::ostream &  o,
const Quaternion3f q 
)
inlinestatic
CollisionRequestFlag hpp::fcl::operator^ ( CollisionRequestFlag  a,
CollisionRequestFlag  b 
)
inline
CollisionRequestFlag& hpp::fcl::operator^= ( CollisionRequestFlag a,
CollisionRequestFlag  b 
)
inline
CollisionRequestFlag hpp::fcl::operator| ( CollisionRequestFlag  a,
CollisionRequestFlag  b 
)
inline
CollisionRequestFlag& hpp::fcl::operator|= ( CollisionRequestFlag a,
CollisionRequestFlag  b 
)
inline
CollisionRequestFlag hpp::fcl::operator~ ( CollisionRequestFlag  a)
inline
bool hpp::fcl::overlap ( const Matrix3f R0,
const Vec3f T0,
const OBB b1,
const OBB b2 
)

Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity.

bool hpp::fcl::overlap ( const Matrix3f R0,
const Vec3f T0,
const OBB b1,
const OBB b2,
const CollisionRequest request,
FCL_REAL sqrDistLowerBound 
)

Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity.

bool hpp::fcl::overlap ( const Matrix3f R0,
const Vec3f T0,
const RSS b1,
const RSS b2 
)

Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.

bool hpp::fcl::overlap ( const Matrix3f R0,
const Vec3f T0,
const kIOS b1,
const kIOS b2 
)

Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity.

Todo:
Not efficient
bool hpp::fcl::overlap ( const Matrix3f R0,
const Vec3f T0,
const RSS b1,
const RSS b2,
const CollisionRequest request,
FCL_REAL sqrDistLowerBound 
)

Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.

bool hpp::fcl::overlap ( const Matrix3f R0,
const Vec3f T0,
const OBBRSS b1,
const OBBRSS b2 
)

Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity.

bool hpp::fcl::overlap ( const Matrix3f R0,
const Vec3f T0,
const kIOS b1,
const kIOS b2,
const CollisionRequest request,
FCL_REAL sqrDistLowerBound 
)

Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity.

Todo:
Not efficient
bool hpp::fcl::overlap ( const Matrix3f R0,
const Vec3f T0,
const OBBRSS b1,
const OBBRSS b2,
const CollisionRequest request,
FCL_REAL sqrDistLowerBound 
)

Check collision between two OBBRSS.

Parameters
b1first OBBRSS in configuration (R0, T0)
b2second OBBRSS in identity position
Return values
squaredlower bound on the distance if OBBRSS do not overlap.
template<size_t N>
bool hpp::fcl::overlap ( const Matrix3f ,
const Vec3f ,
const KDOP< N > &  ,
const KDOP< N > &   
)
template<size_t N>
bool hpp::fcl::overlap ( const Matrix3f ,
const Vec3f ,
const KDOP< N > &  ,
const KDOP< N > &  ,
const CollisionRequest ,
FCL_REAL  
)

References translate().

bool hpp::fcl::overlap ( const Matrix3f R0,
const Vec3f T0,
const AABB b1,
const AABB b2 
)
bool hpp::fcl::overlap ( const Matrix3f R0,
const Vec3f T0,
const AABB b1,
const AABB b2,
const CollisionRequest request,
FCL_REAL sqrDistLowerBound 
)

Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.

void hpp::fcl::propagateBVHFrontListCollisionRecurse ( CollisionTraversalNodeBase node,
const CollisionRequest request,
CollisionResult result,
BVHFrontList front_list 
)

Recurse function for front list propagation.

template<typename Derived , typename OtherDerived >
Derived::Scalar hpp::fcl::quadraticForm ( const Eigen::MatrixBase< Derived > &  R,
const Eigen::MatrixBase< OtherDerived > &  v 
)
template<typename Derived , typename OtherDerived >
void hpp::fcl::relativeTransform ( const Eigen::MatrixBase< Derived > &  R1,
const Eigen::MatrixBase< OtherDerived > &  t1,
const Eigen::MatrixBase< Derived > &  R2,
const Eigen::MatrixBase< OtherDerived > &  t2,
const Eigen::MatrixBase< Derived > &  R,
const Eigen::MatrixBase< OtherDerived > &  t 
)
void hpp::fcl::relativeTransform ( const Transform3f tf1,
const Transform3f tf2,
Transform3f tf 
)

compute the relative transform between two transforms: tf2 = tf1 * tf (relative to the local coordinate system in tf1)

void hpp::fcl::relativeTransform2 ( const Transform3f tf1,
const Transform3f tf2,
Transform3f tf 
)

compute the relative transform between two transforms: tf2 = tf * tf1 (relative to the global coordinate system)

Referenced by hpp::fcl::Transform3f::operator!=().

static AABB hpp::fcl::rotate ( const AABB aabb,
const Matrix3f t 
)
inlinestatic
template<typename Derived >
Derived& hpp::fcl::setEulerYPR ( const Eigen::MatrixBase< Derived > &  R,
FCL_REAL  yaw,
FCL_REAL  pitch,
FCL_REAL  roll 
)
inline

References setEulerZYX().

template<typename Derived >
Derived& hpp::fcl::setEulerZYX ( const Eigen::MatrixBase< Derived > &  R,
FCL_REAL  eulerX,
FCL_REAL  eulerY,
FCL_REAL  eulerZ 
)
inline

Referenced by setEulerYPR().

template<typename T_SH1 , typename T_SH2 , typename NarrowPhaseSolver >
std::size_t hpp::fcl::ShapeShapeCollide ( const CollisionGeometry o1,
const Transform3f tf1,
const CollisionGeometry o2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
const CollisionRequest request,
CollisionResult result 
)
template<typename T_SH1 , typename T_SH2 , typename NarrowPhaseSolver >
FCL_REAL hpp::fcl::ShapeShapeDistance ( const CollisionGeometry o1,
const Transform3f tf1,
const CollisionGeometry o2,
const Transform3f tf2,
const NarrowPhaseSolver *  nsolver,
const DistanceRequest request,
DistanceResult result 
)
Halfspace hpp::fcl::transform ( const Halfspace a,
const Transform3f tf 
)
Plane hpp::fcl::transform ( const Plane a,
const Transform3f tf 
)
OBB hpp::fcl::translate ( const OBB bv,
const Vec3f t 
)

Translate the OBB bv.

RSS hpp::fcl::translate ( const RSS bv,
const Vec3f t 
)

Translate the RSS bv.

kIOS hpp::fcl::translate ( const kIOS bv,
const Vec3f t 
)

Translate the kIOS BV.

OBBRSS hpp::fcl::translate ( const OBBRSS bv,
const Vec3f t 
)

Translate the OBBRSS bv.

template<size_t N>
KDOP<N> hpp::fcl::translate ( const KDOP< N > &  bv,
const Vec3f t 
)

translate the KDOP BV

static AABB hpp::fcl::translate ( const AABB aabb,
const Vec3f t 
)
inlinestatic
template<typename Derived >
static Derived::Scalar hpp::fcl::triple ( const Eigen::MatrixBase< Derived > &  x,
const Eigen::MatrixBase< Derived > &  y,
const Eigen::MatrixBase< Derived > &  z 
)
inlinestatic
void hpp::fcl::updateFrontList ( BVHFrontList front_list,
int  b1,
int  b2 
)
inline

Add new front node into the front list.

References hpp::fcl::BVHFrontNode::BVHFrontNode().