FCL  0.6.0
Flexible Collision Library
fcl Namespace Reference

Main namespace. More...

Namespaces

 time
 Namespace containing time datatypes and time operations.
 

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  BroadPhaseCollisionManager
 Base class for broad phase collision. It helps to accelerate the collision/distance between N objects. Also support self collision, self distance and collision/distance with another M objects. More...
 
class  BroadPhaseContinuousCollisionManager
 Base class for broad phase continuous collision. It helps to accelerate the continuous collision/distance between N objects. Also support self collision, self distance and collision/distance with another M objects. 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  BVMotionBoundVisitor
 Compute the motion bound for a bounding volume, given the closest direction n between two query objects. 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  Capsule
 Center at zero point capsule. 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  Cone
 Center at zero cone. More...
 
struct  constants
 
struct  Contact
 Contact information returned by collision. More...
 
struct  ContactPoint
 Minimal contact information returned by collision. More...
 
class  ContinuousCollisionObject
 the object for continuous collision or distance computation, contains the geometry and the motion information More...
 
struct  ContinuousCollisionRequest
 
struct  ContinuousCollisionResult
 continuous collision result More...
 
class  Convex
 Convex polytope. More...
 
struct  CostSource
 Cost source describes an area with a cost. The area is described by an AABB<S> region. More...
 
class  Cylinder
 Center at zero cylinder. More...
 
struct  DistanceRequest
 request to the distance computation More...
 
struct  DistanceResult
 distance result More...
 
class  DummyCollisionObject
 Dummy collision object with a point AABB<S> More...
 
class  DynamicAABBTreeCollisionManager
 
class  DynamicAABBTreeCollisionManager_Array
 
class  Ellipsoid
 Center at zero point ellipsoid. More...
 
class  Exception
 
struct  GetDistancesImpl
 
struct  GetDistancesImpl< S, 5 >
 
struct  GetDistancesImpl< S, 6 >
 
struct  GetDistancesImpl< S, 9 >
 
struct  GetNodeTypeImpl
 
struct  GetNodeTypeImpl< AABB< S > >
 
struct  GetNodeTypeImpl< KDOP< S, 16 > >
 
struct  GetNodeTypeImpl< KDOP< S, 18 > >
 
struct  GetNodeTypeImpl< KDOP< S, 24 > >
 
struct  GetNodeTypeImpl< kIOS< S > >
 
struct  GetNodeTypeImpl< OBB< S > >
 
struct  GetNodeTypeImpl< OBBRSS< S > >
 
struct  GetNodeTypeImpl< RSS< S > >
 
struct  GetOrientationImpl
 
struct  GetOrientationImpl< S, OBB< S > >
 
struct  GetOrientationImpl< S, OBBRSS< S > >
 
struct  GetOrientationImpl< S, RSS< S > >
 
class  Halfspace
 Half Space: this is equivalent to the Planed in ODE. The separation plane is defined as n * x = d; Points in the negative side of the separation plane (i.e. {x | n * x < d}) are inside the half space and points in the positive side of the separation plane (i.e. {x | n * x > d}) are outside the half space. More...
 
struct  IMatrix3
 
class  InterpMotion
 Linear interpolation motion Each Motion is assumed to have constant linear velocity and angular velocity The motion is R(t)(p - p_ref) + p_ref + T(t) Therefore, R(0) = R0, R(1) = R1 T(0) = T0 + R0 p_ref - p_ref T(1) = T1 + R1 p_ref - p_ref. More...
 
struct  Interval
 Interval class for [a, b]. More...
 
class  IntervalTreeCollisionManager
 Collision manager based on interval tree. More...
 
struct  IVector3
 
class  KDOP
 KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24 The KDOP structure is defined by some pairs of parallel planes defined by some axes. For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 8 (0,-1,0) and (0,1,0) -> indices 1 and 9 (0,0,-1) and (0,0,1) -> indices 2 and 10 (-1,-1,0) and (1,1,0) -> indices 3 and 11 (-1,0,-1) and (1,0,1) -> indices 4 and 12 (0,-1,-1) and (0,1,1) -> indices 5 and 13 (-1,1,0) and (1,-1,0) -> indices 6 and 14 (-1,0,1) and (1,0,-1) -> indices 7 and 15 For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 9 (0,-1,0) and (0,1,0) -> indices 1 and 10 (0,0,-1) and (0,0,1) -> indices 2 and 11 (-1,-1,0) and (1,1,0) -> indices 3 and 12 (-1,0,-1) and (1,0,1) -> indices 4 and 13 (0,-1,-1) and (0,1,1) -> indices 5 and 14 (-1,1,0) and (1,-1,0) -> indices 6 and 15 (-1,0,1) and (1,0,-1) -> indices 7 and 16 (0,-1,1) and (0,1,-1) -> indices 8 and 17 For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 12 (0,-1,0) and (0,1,0) -> indices 1 and 13 (0,0,-1) and (0,0,1) -> indices 2 and 14 (-1,-1,0) and (1,1,0) -> indices 3 and 15 (-1,0,-1) and (1,0,1) -> indices 4 and 16 (0,-1,-1) and (0,1,1) -> indices 5 and 17 (-1,1,0) and (1,-1,0) -> indices 6 and 18 (-1,0,1) and (1,0,-1) -> indices 7 and 19 (0,-1,1) and (0,1,-1) -> indices 8 and 20 (-1, -1, 1) and (1, 1, -1) –> indices 9 and 21 (-1, 1, -1) and (1, -1, 1) –> indices 10 and 22 (1, -1, -1) and (-1, 1, 1) –> indices 11 and 23. More...
 
class  kIOS
 A class describing the kIOS collision structure, which is a set of spheres. More...
 
struct  MakeParentRelativeRecurseImpl
 
struct  MakeParentRelativeRecurseImpl< S, OBB< S > >
 
struct  MakeParentRelativeRecurseImpl< S, OBBRSS< S > >
 
struct  MakeParentRelativeRecurseImpl< S, RSS< S > >
 
class  MotionBase
 
class  NaiveCollisionManager
 Brute force N-body collision manager. More...
 
class  OBB
 Oriented bounding box class. More...
 
class  OBBRSS
 Class merging the OBB and RSS, can handle collision and distance simultaneously. More...
 
class  Plane
 Infinite plane. More...
 
class  RNG
 Random number generation. An instance of this class cannot be used by multiple threads at once (member functions are not const). However, the constructor is thread safe and different instances can be used safely in any number of threads. It is also guaranteed that all created instances will have a different random seed. More...
 
class  RSS
 A class for rectangle sphere-swept bounding volume. More...
 
class  SamplerBase
 
class  SamplerR
 
class  SamplerSE2
 
class  SamplerSE2_disk
 
class  SamplerSE3Euler
 
class  SamplerSE3Euler_ball
 
class  SamplerSE3Quat
 
class  SamplerSE3Quat_ball
 
class  SaPCollisionManager
 Rigorous SAP collision manager. More...
 
class  ScrewMotion
 
class  ShapeBase
 Base class for all basic geometric shapes. More...
 
struct  SortByXLow
 Functor sorting objects according to the AABB<S> lower x bound. More...
 
struct  SortByYLow
 Functor sorting objects according to the AABB<S> lower y bound. More...
 
struct  SortByZLow
 Functor sorting objects according to the AABB<S> lower z bound. More...
 
class  SpatialHashingCollisionManager
 spatial hashing collision mananger More...
 
class  Sphere
 Center at zero point sphere. More...
 
class  SplineMotion
 
class  SSaPCollisionManager
 Simple SAP collision manager. More...
 
class  TaylorModel
 TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function over a time interval, with an interval remainder. All the operations on two Taylor models assume their time intervals are the same. More...
 
class  TBVMotionBoundVisitor
 
struct  TBVMotionBoundVisitorVisitImpl
 
struct  TBVMotionBoundVisitorVisitImpl< S, RSS< S >, InterpMotion< S > >
 Compute the motion bound for a bounding volume along a given direction n according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized) and ci are the endpoints of the generator primitives of RSS. Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame) More...
 
struct  TBVMotionBoundVisitorVisitImpl< S, RSS< S >, ScrewMotion< S > >
 Compute the motion bound for a bounding volume along a given direction n according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized) and ci are the endpoints of the generator primitives of RSS. Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame) More...
 
struct  TBVMotionBoundVisitorVisitImpl< S, RSS< S >, SplineMotion< S > >
 
struct  TBVMotionBoundVisitorVisitImpl< S, RSS< S >, TranslationMotion< S > >
 Compute the motion bound for a bounding volume along a given direction n. More...
 
struct  TimeInterval
 
class  TMatrix3
 
class  TranslationMotion
 
class  Triangle
 Triangle with 3 indices for points. More...
 
class  TriangleMotionBoundVisitor
 
struct  TriangleMotionBoundVisitorVisitImpl
 
struct  TriangleMotionBoundVisitorVisitImpl< S, InterpMotion< S > >
 Compute the motion bound for a triangle along a given direction n according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||R0(ci) x w|| / |w|. w is the angular velocity and ci are the triangle vertex coordinates. Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame) More...
 
struct  TriangleMotionBoundVisitorVisitImpl< S, ScrewMotion< S > >
 Compute the motion bound for a triangle along a given direction n according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||R0(ci) x w|| / |w|. w is the angular velocity and ci are the triangle vertex coordinates. Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame) More...
 
struct  TriangleMotionBoundVisitorVisitImpl< S, SplineMotion< S > >
 
struct  TriangleMotionBoundVisitorVisitImpl< S, TranslationMotion< S > >
 Compute the motion bound for a triangle along a given direction n. More...
 
class  TriangleP
 Triangle stores the points instead of only indices of points. More...
 
class  TVector3
 
class  Variance3
 Class for variance matrix in 3d. More...
 

Typedefs

using NaiveCollisionManagerf = NaiveCollisionManager< float >
 
using NaiveCollisionManagerd = NaiveCollisionManager< double >
 
template<typename S >
using CollisionCallBack = bool(*)(CollisionObject< S > *o1, CollisionObject< S > *o2, void *cdata)
 Callback for collision between two objects. Return value is whether can stop now.
 
template<typename S >
using DistanceCallBack = bool(*)(CollisionObject< S > *o1, CollisionObject< S > *o2, void *cdata, S &dist)
 Callback for distance between two objects, Return value is whether can stop now, also return the minimum distance till now.
 
using BroadPhaseCollisionManagerf = BroadPhaseCollisionManager< float >
 
using BroadPhaseCollisionManagerd = BroadPhaseCollisionManager< double >
 
template<typename S >
using ContinuousCollisionCallBack = bool(*)(ContinuousCollisionObject< S > *o1, ContinuousCollisionObject< S > *o2, void *cdata)
 Callback for continuous collision between two objects. Return value is whether can stop now.
 
template<typename S >
using ContinuousDistanceCallBack = bool(*)(ContinuousCollisionObject< S > *o1, ContinuousCollisionObject< S > *o2, void *cdata, S &dist)
 Callback for continuous distance between two objects, Return value is whether can stop now, also return the minimum distance till now.
 
using BroadPhaseContinuousCollisionManagerf = BroadPhaseContinuousCollisionManager< float >
 
using BroadPhaseContinuousCollisionManagerd = BroadPhaseContinuousCollisionManager< double >
 
using DynamicAABBTreeCollisionManagerf = DynamicAABBTreeCollisionManager< float >
 
using DynamicAABBTreeCollisionManagerd = DynamicAABBTreeCollisionManager< double >
 
using DynamicAABBTreeCollisionManager_Arrayf = DynamicAABBTreeCollisionManager_Array< float >
 
using DynamicAABBTreeCollisionManager_Arrayd = DynamicAABBTreeCollisionManager_Array< double >
 
using IntervalTreeCollisionManagerf = IntervalTreeCollisionManager< float >
 
using IntervalTreeCollisionManagerd = IntervalTreeCollisionManager< double >
 
using SaPCollisionManagerf = SaPCollisionManager< float >
 
using SaPCollisionManagerd = SaPCollisionManager< double >
 
template<typename HashTable = detail::SimpleHashTable<AABB<float>, CollisionObject<float>*, detail::SpatialHash<float>>>
using SpatialHashingCollisionManagerf = SpatialHashingCollisionManager< float, HashTable >
 
template<typename HashTable = detail::SimpleHashTable<AABB<double>, CollisionObject<double>*, detail::SpatialHash<double>>>
using SpatialHashingCollisionManagerd = SpatialHashingCollisionManager< double, HashTable >
 
using SSaPCollisionManagerf = SSaPCollisionManager< float >
 
using SSaPCollisionManagerd = SSaPCollisionManager< double >
 
typedef FCL_DEPRECATED double FCL_REAL
 
typedef FCL_DEPRECATED std::int64_t FCL_INT64
 
typedef FCL_DEPRECATED std::uint64_t FCL_UINT64
 
typedef FCL_DEPRECATED std::int32_t FCL_INT32
 
typedef FCL_DEPRECATED std::uint32_t FCL_UINT32
 
using int64 = std::int64_t
 
using uint64 = std::uint64_t
 
using int32 = std::int32_t
 
using uint32 = std::uint32_t
 
template<typename S >
using Vector2 = Eigen::Matrix< S, 2, 1 >
 
template<typename S >
using Vector3 = Eigen::Matrix< S, 3, 1 >
 
template<typename S >
using Vector6 = Eigen::Matrix< S, 6, 1 >
 
template<typename S >
using Vector7 = Eigen::Matrix< S, 7, 1 >
 
template<typename S , int N>
using VectorN = Eigen::Matrix< S, N, 1 >
 
template<typename S >
using VectorX = Eigen::Matrix< S, Eigen::Dynamic, 1 >
 
template<typename S >
using Matrix3 = Eigen::Matrix< S, 3, 3 >
 
template<typename S >
using Quaternion = Eigen::Quaternion< S >
 
template<typename S >
using Transform3 = Eigen::Transform< S, 3, Eigen::AffineCompact >
 
template<typename S >
using Translation3 = Eigen::Translation< S, 3 >
 
template<typename S >
using AngleAxis = Eigen::AngleAxis< S >
 
using Vector3f = Vector3< float >
 
template<int N>
using VectorNf = VectorN< float, N >
 
using VectorXf = VectorX< float >
 
using Matrix3f = Matrix3< float >
 
using Quaternionf = Quaternion< float >
 
using Transform3f = Transform3< float >
 
using Translation3f = Translation3< float >
 
using AngleAxisf = AngleAxis< float >
 
using Vector3d = Vector3< double >
 
template<int N>
using VectorNd = VectorN< double, N >
 
using VectorXd = VectorX< double >
 
using Matrix3d = Matrix3< double >
 
using Quaterniond = Quaternion< double >
 
using Transform3d = Transform3< double >
 
using Translation3d = Translation3< double >
 
using AngleAxisd = AngleAxis< double >
 
using CollisionGeometryf = CollisionGeometry< float >
 
using CollisionGeometryd = CollisionGeometry< double >
 
using Boxf = Box< float >
 
using Boxd = Box< double >
 
using Capsulef = Capsule< float >
 
using Capsuled = Capsule< double >
 
using Conef = Cone< float >
 
using Coned = Cone< double >
 
using Convexf = Convex< float >
 
using Convexd = Convex< double >
 
using Cylinderf = Cylinder< float >
 
using Cylinderd = Cylinder< double >
 
using Ellipsoidf = Ellipsoid< float >
 
using Ellipsoidd = Ellipsoid< double >
 
using Halfspacef = Halfspace< float >
 
using Halfspaced = Halfspace< double >
 
using Planef = Plane< float >
 
using Planed = Plane< double >
 
using ShapeBasef = ShapeBase< float >
 
using ShapeBased = ShapeBase< double >
 
using Spheref = Sphere< float >
 
using Sphered = Sphere< double >
 
using TrianglePf = TriangleP< float >
 
using TrianglePd = TriangleP< double >
 
using AABBf = AABB< float >
 
using AABBd = AABB< double >
 
template<std::size_t N>
using KDOPf = KDOP< float, N >
 
template<std::size_t N>
using KDOPd = KDOP< double, N >
 
using kIOSf = kIOS< float >
 
using kIOSd = kIOS< double >
 
using OBBf = OBB< float >
 
using OBBd = OBB< double >
 
using OBBRSSf = OBBRSS< float >
 
using OBBRSSd = OBBRSS< double >
 
using RSSf = RSS< float >
 
using RSSd = RSS< double >
 
using constantsf = constants< float >
 
using constantsd = constants< double >
 
using MotionBasef = MotionBase< float >
 
using MotionBased = MotionBase< double >
 
template<typename S >
using MotionBasePtr = std::shared_ptr< MotionBase< S >>
 
using TranslationMotionf = TranslationMotion< float >
 
using TranslationMotiond = TranslationMotion< double >
 
using RNGf = RNG< float >
 
using RNGd = RNG< double >
 
template<std::size_t N>
using SamplerRf = SamplerR< float, N >
 
template<std::size_t N>
using SamplerRd = SamplerR< double, N >
 
using SamplerSE2f = SamplerSE2< float >
 
using SamplerSE2d = SamplerSE2< double >
 
using SamplerSE2_diskf = SamplerSE2_disk< float >
 
using SamplerSE2_diskd = SamplerSE2_disk< double >
 
using SamplerSE3Eulerf = SamplerSE3Euler< float >
 
using SamplerSE3Eulerd = SamplerSE3Euler< double >
 
using SamplerSE3Euler_ballf = SamplerSE3Euler_ball< float >
 
using SamplerSE3Euler_balld = SamplerSE3Euler_ball< double >
 
using SamplerSE3Quatf = SamplerSE3Quat< float >
 
using SamplerSE3Quatd = SamplerSE3Quat< double >
 
using SamplerSE3Quat_ballf = SamplerSE3Quat_ball< float >
 
using SamplerSE3Quat_balld = SamplerSE3Quat_ball< double >
 
using Variance3f = Variance3< float >
 
using Variance3d = Variance3< double >
 
using CollisionObjectf = CollisionObject< float >
 
using CollisionObjectd = CollisionObject< double >
 
using CollisionRequestf = CollisionRequest< float >
 
using CollisionRequestd = CollisionRequest< double >
 
using CollisionResultf = CollisionResult< float >
 
using CollisionResultd = CollisionResult< double >
 
using Contactf = Contact< float >
 
using Contactd = Contact< double >
 
using ContactPointf = ContactPoint< float >
 
using ContactPointd = ContactPoint< double >
 
using ContinuousCollisionObjectf = ContinuousCollisionObject< float >
 
using ContinuousCollisionObjectd = ContinuousCollisionObject< double >
 
using ContinuousCollisionRequestf = ContinuousCollisionRequest< float >
 
using ContinuousCollisionRequestd = ContinuousCollisionRequest< double >
 
using ContinuousCollisionResultf = ContinuousCollisionResult< float >
 
using ContinuousCollisionResultd = ContinuousCollisionResult< double >
 
using CostSourcef = CostSource< float >
 
using CostSourced = CostSource< double >
 
using DistanceRequestf = DistanceRequest< float >
 
using DistanceRequestd = DistanceRequest< double >
 
using DistanceResultf = DistanceResult< float >
 
using DistanceResultd = DistanceResult< double >
 

Enumerations

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 -> ...... | |-> update_begun -> updated -> ..... 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  OBJECT_TYPE {
  OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE,
  OT_COUNT
}
 object type: BVH (mesh, points), basic geometry, 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
}
 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  CCDMotionType { CCDM_TRANS, CCDM_LINEAR, CCDM_SCREW, CCDM_SPLINE }
 
enum  CCDSolverType { CCDC_NAIVE, CCDC_CONSERVATIVE_ADVANCEMENT, CCDC_RAY_SHOOTING, CCDC_POLYNOMIAL_SOLVER }
 
enum  GJKSolverType { GST_LIBCCD, GST_INDEP }
 Type of narrow phase GJK solver.
 

Functions

template void BVHExpand (BVHModel< OBB< double >> &model, const Variance3< double > *ucs, double r)
 
template void BVHExpand (BVHModel< RSS< double >> &model, const Variance3< double > *ucs, double r)
 
template<typename S , typename BV >
void BVHExpand (BVHModel< BV > &model, const Variance3< S > *ucs, S r)
 Expand the BVH bounding boxes according to the variance matrix corresponding to the data stored within each BV node.
 
template<typename S >
void BVHExpand (BVHModel< OBB< S >> &model, const Variance3< S > *ucs, S r=1.0)
 Expand the BVH bounding boxes according to the corresponding variance information, for OBB.
 
template<typename S >
void BVHExpand (BVHModel< RSS< S >> &model, const Variance3< S > *ucs, S r=1.0)
 Expand the BVH bounding boxes according to the corresponding variance information, for RSS.
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Box< typename BV::S > &shape, const Transform3< typename BV::S > &pose)
 Generate BVH model from box.
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Sphere< typename BV::S > &shape, const Transform3< typename BV::S > &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.
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Sphere< typename BV::S > &shape, const Transform3< typename BV::S > &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.
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Ellipsoid< typename BV::S > &shape, const Transform3< typename BV::S > &pose, unsigned int seg, unsigned int ring)
 Generate BVH model from ellipsoid, given the number of segments along longitude and number of rings along latitude.
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Ellipsoid< typename BV::S > &shape, const Transform3< typename BV::S > &pose, unsigned int n_faces_for_unit_ellipsoid)
 Generate BVH model from ellipsoid The difference between generateBVHModel is that it gives the number of triangles faces N for an ellipsoid with unit radii (1, 1, 1). For ellipsoid of radii (a, b, c), then the number of triangles is ((a^p * b^p + b^p * c^p + c^p * a^p)/3)^(1/p) * N, where p is 1.6075, so that the area represented by a single triangle is approximately the same. Reference: https://en.wikipedia.org/wiki/Ellipsoid<S>#Approximate_formula.
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Cylinder< typename BV::S > &shape, const Transform3< typename BV::S > &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.
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Cylinder< typename BV::S > &shape, const Transform3< typename BV::S > &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.
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Cone< typename BV::S > &shape, const Transform3< typename BV::S > &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.
 
template<typename BV >
void generateBVHModel (BVHModel< BV > &model, const Cone< typename BV::S > &shape, const Transform3< typename BV::S > &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.
 
template Halfspace< double > transform (const Halfspace< double > &a, const Transform3< double > &tf)
 
template<typename S >
Halfspace< S > transform (const Halfspace< S > &a, const Transform3< S > &tf)
 
template Plane< double > transform (const Plane< double > &a, const Transform3< double > &tf)
 
template<typename S >
Plane< S > transform (const Plane< S > &a, const Transform3< S > &tf)
 
template void constructBox (const OBB< double > &bv, Box< double > &box, Transform3< double > &tf)
 
template void constructBox (const OBBRSS< double > &bv, Box< double > &box, Transform3< double > &tf)
 
template void constructBox (const kIOS< double > &bv, Box< double > &box, Transform3< double > &tf)
 
template void constructBox (const RSS< double > &bv, Box< double > &box, Transform3< double > &tf)
 
template void constructBox (const KDOP< double, 16 > &bv, Box< double > &box, Transform3< double > &tf)
 
template void constructBox (const KDOP< double, 18 > &bv, Box< double > &box, Transform3< double > &tf)
 
template void constructBox (const KDOP< double, 24 > &bv, Box< double > &box, Transform3< double > &tf)
 
template void constructBox (const AABB< double > &bv, const Transform3< double > &tf_bv, Box< double > &box, Transform3< double > &tf)
 
template void constructBox (const OBB< double > &bv, const Transform3< double > &tf_bv, Box< double > &box, Transform3< double > &tf)
 
template void constructBox (const OBBRSS< double > &bv, const Transform3< double > &tf_bv, Box< double > &box, Transform3< double > &tf)
 
template void constructBox (const kIOS< double > &bv, const Transform3< double > &tf_bv, Box< double > &box, Transform3< double > &tf)
 
template void constructBox (const RSS< double > &bv, const Transform3< double > &tf_bv, Box< double > &box, Transform3< double > &tf)
 
template void constructBox (const KDOP< double, 16 > &bv, const Transform3< double > &tf_bv, Box< double > &box, Transform3< double > &tf)
 
template void constructBox (const KDOP< double, 18 > &bv, const Transform3< double > &tf_bv, Box< double > &box, Transform3< double > &tf)
 
template void constructBox (const KDOP< double, 24 > &bv, const Transform3< double > &tf_bv, Box< double > &box, Transform3< double > &tf)
 
template<typename BV , typename Shape >
void computeBV (const Shape &s, const Transform3< typename BV::S > &tf, BV &bv)
 calculate a bounding volume for a shape in a specific configuration
 
template<typename S >
void constructBox (const AABB< S > &bv, Box< S > &box, Transform3< S > &tf)
 construct a box shape (with a configuration) from a given bounding volume
 
template<typename S >
void constructBox (const OBB< S > &bv, Box< S > &box, Transform3< S > &tf)
 
template<typename S >
void constructBox (const OBBRSS< S > &bv, Box< S > &box, Transform3< S > &tf)
 
template<typename S >
void constructBox (const kIOS< S > &bv, Box< S > &box, Transform3< S > &tf)
 
template<typename S >
void constructBox (const RSS< S > &bv, Box< S > &box, Transform3< S > &tf)
 
template<typename S >
void constructBox (const KDOP< S, 16 > &bv, Box< S > &box, Transform3< S > &tf)
 
template<typename S >
void constructBox (const KDOP< S, 18 > &bv, Box< S > &box, Transform3< S > &tf)
 
template<typename S >
void constructBox (const KDOP< S, 24 > &bv, Box< S > &box, Transform3< S > &tf)
 
template<typename S >
void constructBox (const AABB< S > &bv, const Transform3< S > &tf_bv, Box< S > &box, Transform3< S > &tf)
 
template<typename S >
void constructBox (const OBB< S > &bv, const Transform3< S > &tf_bv, Box< S > &box, Transform3< S > &tf)
 
template<typename S >
void constructBox (const OBBRSS< S > &bv, const Transform3< S > &tf_bv, Box< S > &box, Transform3< S > &tf)
 
template<typename S >
void constructBox (const kIOS< S > &bv, const Transform3< S > &tf_bv, Box< S > &box, Transform3< S > &tf)
 
template<typename S >
void constructBox (const RSS< S > &bv, const Transform3< S > &tf_bv, Box< S > &box, Transform3< S > &tf)
 
template<typename S >
void constructBox (const KDOP< S, 16 > &bv, const Transform3< S > &tf_bv, Box< S > &box, Transform3< S > &tf)
 
template<typename S >
void constructBox (const KDOP< S, 18 > &bv, const Transform3< S > &tf_bv, Box< S > &box, Transform3< S > &tf)
 
template<typename S >
void constructBox (const KDOP< S, 24 > &bv, const Transform3< S > &tf_bv, Box< S > &box, Transform3< S > &tf)
 
template<typename S , typename Derived >
AABB< S > translate (const AABB< S > &aabb, const Eigen::MatrixBase< Derived > &t)
 translate the center of AABB by t
 
template void minmax (double a, double b, double &minv, double &maxv)
 
template void minmax (double p, double &minv, double &maxv)
 
template void getDistances< double, 5 > (const Vector3< double > &p, double *d)
 
template void getDistances< double, 6 > (const Vector3< double > &p, double *d)
 
template void getDistances< double, 9 > (const Vector3< double > &p, double *d)
 
template<typename S , std::size_t N, typename Derived >
KDOP< S, N > translate (const KDOP< S, N > &bv, const Eigen::MatrixBase< Derived > &t)
 translate the KDOP BV
 
template<typename S >
void minmax (S a, S b, S &minv, S &maxv)
 Find the smaller and larger one of two values.
 
template<typename S >
void minmax (S p, S &minv, S &maxv)
 Merge the interval [minv, maxv] and value p/.
 
template<typename S , std::size_t N>
void getDistances (const Vector3< S > &p, S *d)
 Compute the distances to planes with normals from KDOP vectors except those of AABB face planes.
 
template<typename S , typename DerivedA , typename DerivedB >
bool overlap (const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2)
 Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity. More...
 
template<typename S , typename DerivedA , typename DerivedB >
distance (const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P=nullptr, Vector3< S > *Q=nullptr)
 Approximate distance between two kIOS bounding volumes. More...
 
template<typename S >
distance (const Transform3< S > &tf, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P=nullptr, Vector3< S > *Q=nullptr)
 Approximate distance between two kIOS bounding volumes. More...
 
template<typename S , typename Derived >
kIOS< S > translate (const kIOS< S > &bv, const Eigen::MatrixBase< Derived > &t)
 Translate the kIOS BV.
 
template<typename S >
bool overlap (const Transform3< S > &tf, const kIOS< S > &b1, const kIOS< S > &b2)
 Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity. More...
 
template void computeVertices (const OBB< double > &b, Vector3< double > vertices[8])
 
template OBB< double > merge_largedist (const OBB< double > &b1, const OBB< double > &b2)
 
template OBB< double > merge_smalldist (const OBB< double > &b1, const OBB< double > &b2)
 
template bool obbDisjoint (const Matrix3< double > &B, const Vector3< double > &T, const Vector3< double > &a, const Vector3< double > &b)
 
template bool obbDisjoint (const Transform3< double > &tf, const Vector3< double > &a, const Vector3< double > &b)
 
template<typename S >
void computeVertices (const OBB< S > &b, Vector3< S > vertices[8])
 Compute the 8 vertices of a OBB.
 
template<typename S >
OBB< S > merge_largedist (const OBB< S > &b1, const OBB< S > &b2)
 OBB merge method when the centers of two smaller OBB are far away.
 
template<typename S >
OBB< S > merge_smalldist (const OBB< S > &b1, const OBB< S > &b2)
 OBB merge method when the centers of two smaller OBB are close.
 
template<typename S , typename Derived >
OBB< S > translate (const OBB< S > &bv, const Eigen::MatrixBase< Derived > &t)
 Translate the OBB bv.
 
template<typename S , typename DerivedA , typename DerivedB >
bool overlap (const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const OBB< S > &b1, const OBB< S > &b2)
 Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity.
 
template<typename S >
bool obbDisjoint (const Matrix3< S > &B, const Vector3< S > &T, const Vector3< S > &a, const Vector3< S > &b)
 Check collision between two boxes: the first box is in configuration (R, T) and its half dimension is set by a; the second box is in identity configuration and its half dimension is set by b.
 
template<typename S >
bool obbDisjoint (const Transform3< S > &tf, const Vector3< S > &a, const Vector3< S > &b)
 Check collision between two boxes: the first box is in configuration (R, T) and its half dimension is set by a; the second box is in identity configuration and its half dimension is set by b.
 
template OBBRSS< double > translate (const OBBRSS< double > &bv, const Vector3< double > &t)
 
template<typename S , typename DerivedA , typename DerivedB >
bool overlap (const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const OBBRSS< S > &b1, const OBBRSS< S > &b2)
 Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity.
 
template<typename S , typename DerivedA , typename DerivedB >
distance (const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const OBBRSS< S > &b1, const OBBRSS< S > &b2, Vector3< S > *P=nullptr, Vector3< S > *Q=nullptr)
 Computate distance between two OBBRSS, b1 is in configuation (R0, T0) and b2 is in indentity; P and Q, is not nullptr, returns the nearest points.
 
template<typename S >
OBBRSS< S > translate (const OBBRSS< S > &bv, const Vector3< S > &t)
 Translate the OBBRSS bv.
 
template<typename S >
bool overlap (const Transform3< S > &tf, const OBBRSS< S > &b1, const OBBRSS< S > &b2)
 Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity.
 
template<typename S >
distance (const Transform3< S > &tf, const OBBRSS< S > &b1, const OBBRSS< S > &b2, Vector3< S > *P=nullptr, Vector3< S > *Q=nullptr)
 Computate distance between two OBBRSS, b1 is in configuation (R0, T0) and b2 is in indentity; P and Q, is not nullptr, returns the nearest points.
 
template void clipToRange (double &val, double a, double b)
 
template void segCoords (double &t, double &u, double a, double b, double A_dot_B, double A_dot_T, double B_dot_T)
 
template bool inVoronoi (double a, double b, double Anorm_dot_B, double Anorm_dot_T, double A_dot_B, double A_dot_T, double B_dot_T)
 
template double rectDistance (const Matrix3< double > &Rab, const Vector3< double > &Tab, const double a[2], const double b[2], Vector3< double > *P, Vector3< double > *Q)
 
template double rectDistance (const Transform3< double > &tfab, const double a[2], const double b[2], Vector3< double > *P, Vector3< double > *Q)
 
template RSS< double > translate (const RSS< double > &bv, const Vector3< double > &t)
 
template<typename S >
void clipToRange (S &val, S a, S b)
 Clip value between a and b.
 
template<typename S >
void segCoords (S &t, S &u, S a, S b, S A_dot_B, S A_dot_T, S B_dot_T)
 Finds the parameters t & u corresponding to the two closest points on a pair of line segments. The first segment is defined as Pa + A*t, 0 <= t <= a, where "Pa" is one endpoint of the segment, "A" is a unit vector pointing to the other endpoint, and t is a scalar that produces all the points between the two endpoints. Since "A" is a unit vector, "a" is the segment's length. The second segment is defined as Pb + B*u, 0 <= u <= b. Many of the terms needed by the algorithm are already computed for other purposes,so we just pass these terms into the function instead of complete specifications of each segment. "T" in the dot products is the vector betweeen Pa and Pb. Reference: "On fast computation of distance between line segments." Vladimir J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985.
 
template<typename S >
bool inVoronoi (S a, S b, S Anorm_dot_B, S Anorm_dot_T, S A_dot_B, S A_dot_T, S B_dot_T)
 Returns whether the nearest point on rectangle edge Pb + B*u, 0 <= u <= b, to the rectangle edge, Pa + A*t, 0 <= t <= a, is within the half space determined by the point Pa and the direction Anorm. A,B, and Anorm are unit vectors. T is the vector between Pa and Pb.
 
template<typename S >
rectDistance (const Matrix3< S > &Rab, const Vector3< S > &Tab, const S a[2], const S b[2], Vector3< S > *P=nullptr, Vector3< S > *Q=nullptr)
 Distance between two oriented rectangles; P and Q (optional return values) are the closest points in the rectangles, both are in the local frame of the first rectangle.
 
template<typename S >
rectDistance (const Transform3< S > &tfab, const S a[2], const S b[2], Vector3< S > *P=nullptr, Vector3< S > *Q=nullptr)
 Distance between two oriented rectangles; P and Q (optional return values) are the closest points in the rectangles, both are in the local frame of the first rectangle.
 
template<typename S , typename DerivedA , typename DerivedB >
bool overlap (const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const RSS< S > &b1, const RSS< S > &b2)
 Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
 
template<typename S , typename DerivedA , typename DerivedB >
distance (const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const RSS< S > &b1, const RSS< S > &b2, Vector3< S > *P=nullptr, Vector3< S > *Q=nullptr)
 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)
 
template<typename S >
RSS< S > translate (const RSS< S > &bv, const Vector3< S > &t)
 Translate the RSS bv.
 
template<typename BV >
void fit (Vector3< typename BV::S > *ps, int n, BV &bv)
 Compute a bounding volume that fits a set of n points.
 
template<typename BV1 , typename BV2 >
void convertBV (const BV1 &bv1, const Transform3< typename BV1::S > &tf1, BV2 &bv2)
 Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity configuration.
 
template void normalize (Vector3d &v, bool *signal)
 
template void hat (Matrix3d &mat, const Vector3d &vec)
 
template void eigen (const Matrix3d &m, Vector3d &dout, Matrix3d &vout)
 
template void eigen_old (const Matrix3d &m, Vector3d &dout, Matrix3d &vout)
 
template void axisFromEigen (const Matrix3d &eigenV, const Vector3d &eigenS, Matrix3d &axis)
 
template void axisFromEigen (const Matrix3d &eigenV, const Vector3d &eigenS, Transform3d &tf)
 
template void generateCoordinateSystem (Matrix3d &axis)
 
template void generateCoordinateSystem (Transform3d &tf)
 
template void getRadiusAndOriginAndRectangleSize (Vector3d *ps, Vector3d *ps2, Triangle *ts, unsigned int *indices, int n, const Matrix3d &axis, Vector3d &origin, double l[2], double &r)
 
template void getRadiusAndOriginAndRectangleSize (Vector3d *ps, Vector3d *ps2, Triangle *ts, unsigned int *indices, int n, Transform3d &tf, double l[2], double &r)
 
template void circumCircleComputation (const Vector3d &a, const Vector3d &b, const Vector3d &c, Vector3d &center, double &radius)
 
template double maximumDistance (Vector3d *ps, Vector3d *ps2, Triangle *ts, unsigned int *indices, int n, const Vector3d &query)
 
template void getExtentAndCenter (Vector3d *ps, Vector3d *ps2, Triangle *ts, unsigned int *indices, int n, const Matrix3d &axis, Vector3d &center, Vector3d &extent)
 
template void getCovariance (Vector3d *ps, Vector3d *ps2, Triangle *ts, unsigned int *indices, int n, Matrix3d &M)
 
template<typename S >
void normalize (Vector3< S > &v, bool *signal)
 
template<typename Derived >
Derived::RealScalar triple (const Eigen::MatrixBase< Derived > &x, const Eigen::MatrixBase< Derived > &y, const Eigen::MatrixBase< Derived > &z)
 
template<typename Derived >
void generateCoordinateSystem (const Eigen::MatrixBase< Derived > &w, Eigen::MatrixBase< Derived > &u, Eigen::MatrixBase< Derived > &v)
 
template<typename S , int M, int N>
VectorN< S, M+N > combine (const VectorN< S, M > &v1, const VectorN< S, N > &v2)
 
template<typename S >
void hat (Matrix3< S > &mat, const Vector3< S > &vec)
 
template<typename S >
void eigen (const Matrix3< S > &m, Vector3< S > &dout, Matrix3< S > &vout)
 compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen vectors
 
template<typename S >
void eigen_old (const Matrix3< S > &m, Vector3< S > &dout, Matrix3< S > &vout)
 compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen vectors
 
template<typename S >
void axisFromEigen (const Matrix3< S > &eigenV, const Vector3< S > &eigenS, Matrix3< S > &axis)
 
template<typename S >
void axisFromEigen (const Matrix3< S > &eigenV, const Vector3< S > &eigenS, Transform3< S > &tf)
 
template<typename S >
void generateCoordinateSystem (Matrix3< S > &axis)
 
template<typename S >
void generateCoordinateSystem (Transform3< S > &tf)
 
template<typename DerivedA , typename DerivedB , typename DerivedC , typename DerivedD >
void relativeTransform (const Eigen::MatrixBase< DerivedA > &R1, const Eigen::MatrixBase< DerivedB > &t1, const Eigen::MatrixBase< DerivedA > &R2, const Eigen::MatrixBase< DerivedB > &t2, Eigen::MatrixBase< DerivedC > &R, Eigen::MatrixBase< DerivedD > &t)
 
template<typename S , typename DerivedA , typename DerivedB >
void relativeTransform (const Transform3< S > &T1, const Transform3< S > &T2, Eigen::MatrixBase< DerivedA > &R, Eigen::MatrixBase< DerivedB > &t)
 
template<typename S >
void getRadiusAndOriginAndRectangleSize (Vector3< S > *ps, Vector3< S > *ps2, Triangle *ts, unsigned int *indices, int n, const Matrix3< S > &axis, Vector3< S > &origin, S l[2], S &r)
 Compute the RSS bounding volume parameters: radius, rectangle size and the origin, given the BV axises.
 
template<typename S >
void getRadiusAndOriginAndRectangleSize (Vector3< S > *ps, Vector3< S > *ps2, Triangle *ts, unsigned int *indices, int n, Transform3< S > &tf, S l[2], S &r)
 Compute the RSS bounding volume parameters: radius, rectangle size and the origin, given the BV axises.
 
template<typename S >
void circumCircleComputation (const Vector3< S > &a, const Vector3< S > &b, const Vector3< S > &c, Vector3< S > &center, S &radius)
 Compute the center and radius for a triangle's circumcircle.
 
template<typename S >
maximumDistance (Vector3< S > *ps, Vector3< S > *ps2, Triangle *ts, unsigned int *indices, int n, const Vector3< S > &query)
 Compute the maximum distance from a given center point to a point cloud.
 
template<typename S >
void getExtentAndCenter (Vector3< S > *ps, Vector3< S > *ps2, Triangle *ts, unsigned int *indices, int n, const Matrix3< S > &axis, Vector3< S > &center, Vector3< S > &extent)
 Compute the bounding volume extent and center for a set or subset of points, given the BV axises.
 
template<typename S >
void getCovariance (Vector3< S > *ps, Vector3< S > *ps2, Triangle *ts, unsigned int *indices, int n, Matrix3< S > &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.
 
template<typename S , typename DerivedA , typename DerivedB >
void relativeTransform (const Eigen::Transform< S, 3, Eigen::Isometry > &T1, const Eigen::Transform< S, 3, Eigen::Isometry > &T2, Eigen::MatrixBase< DerivedA > &R, Eigen::MatrixBase< DerivedB > &t)
 
template<typename S >
void getExtentAndCenter (Vector3< S > *ps, Vector3< S > *ps2, Triangle *ts, unsigned int *indices, int n, Transform3< S > &tf, Vector3< S > &extent)
 Compute the bounding volume extent and center for a set or subset of points, given the BV axises.
 
template Interval< double > bound (const Interval< double > &i, double v)
 
template Interval< double > bound (const Interval< double > &i, const Interval< double > &other)
 
template<typename S >
Interval< S > bound (const Interval< S > &i, S v)
 
template<typename S >
Interval< S > bound (const Interval< S > &i, const Interval< S > &other)
 
template IMatrix3< double > rotationConstrain (const IMatrix3< double > &m)
 
template<typename S >
IMatrix3< S > rotationConstrain (const IMatrix3< S > &m)
 
template IVector3< double > bound (const IVector3< double > &i, const Vector3< double > &v)
 
template IVector3< double > bound (const IVector3< double > &i, const IVector3< double > &v)
 
template<typename S >
IVector3< S > bound (const IVector3< S > &i, const IVector3< S > &v)
 
template<typename S >
IVector3< S > bound (const IVector3< S > &i, const Vector3< S > &v)
 
template TMatrix3< double > rotationConstrain (const TMatrix3< double > &m)
 
template TMatrix3< double > operator* (const Matrix3< double > &m, const TaylorModel< double > &a)
 
template TMatrix3< double > operator* (const TaylorModel< double > &a, const Matrix3< double > &m)
 
template TMatrix3< double > operator* (const TaylorModel< double > &a, const TMatrix3< double > &m)
 
template TMatrix3< double > operator* (double d, const TMatrix3< double > &m)
 
template TMatrix3< double > operator+ (const Matrix3< double > &m1, const TMatrix3< double > &m2)
 
template TMatrix3< double > operator- (const Matrix3< double > &m1, const TMatrix3< double > &m2)
 
template<typename S >
TMatrix3< S > rotationConstrain (const TMatrix3< S > &m)
 
template<typename S >
TMatrix3< S > operator* (const Matrix3< S > &m, const TaylorModel< S > &a)
 
template<typename S >
TMatrix3< S > operator* (const TaylorModel< S > &a, const Matrix3< S > &m)
 
template<typename S >
TMatrix3< S > operator* (const TaylorModel< S > &a, const TMatrix3< S > &m)
 
template<typename S >
TMatrix3< S > operator* (S d, const TMatrix3< S > &m)
 
template<typename S >
TMatrix3< S > operator+ (const Matrix3< S > &m1, const TMatrix3< S > &m2)
 
template<typename S >
TMatrix3< S > operator- (const Matrix3< S > &m1, const TMatrix3< S > &m2)
 
template TaylorModel< double > operator* (double d, const TaylorModel< double > &a)
 
template TaylorModel< double > operator+ (double d, const TaylorModel< double > &a)
 
template TaylorModel< double > operator- (double d, const TaylorModel< double > &a)
 
template void generateTaylorModelForCosFunc (TaylorModel< double > &tm, double w, double q0)
 
template void generateTaylorModelForSinFunc (TaylorModel< double > &tm, double w, double q0)
 
template void generateTaylorModelForLinearFunc (TaylorModel< double > &tm, double p, double v)
 
template<typename S >
TaylorModel< S > operator* (S d, const TaylorModel< S > &a)
 
template<typename S >
TaylorModel< S > operator+ (S d, const TaylorModel< S > &a)
 
template<typename S >
TaylorModel< S > operator- (S d, const TaylorModel< S > &a)
 
template<typename S >
void generateTaylorModelForCosFunc (TaylorModel< S > &tm, S w, S q0)
 Generate Taylor model for cos(w t + q0)
 
template<typename S >
void generateTaylorModelForSinFunc (TaylorModel< S > &tm, S w, S q0)
 Generate Taylor model for sin(w t + q0)
 
template<typename S >
void generateTaylorModelForLinearFunc (TaylorModel< S > &tm, S p, S v)
 Generate Taylor model for p + v t.
 
template void generateTVector3ForLinearFunc (TVector3< double > &v, const Vector3< double > &position, const Vector3< double > &velocity)
 
template TVector3< double > operator* (const Vector3< double > &v, const TaylorModel< double > &a)
 
template TVector3< double > operator+ (const Vector3< double > &v1, const TVector3< double > &v2)
 
template TVector3< double > operator- (const Vector3< double > &v1, const TVector3< double > &v2)
 
template<typename S >
void generateTVector3ForLinearFunc (TVector3< S > &v, const Vector3< S > &position, const Vector3< S > &velocity)
 
template<typename S >
TVector3< S > operator* (const Vector3< S > &v, const TaylorModel< S > &a)
 
template<typename S >
TVector3< S > operator+ (const Vector3< S > &v1, const TVector3< S > &v2)
 
template<typename S >
TVector3< S > operator- (const Vector3< S > &v1, const TVector3< S > &v2)
 
template std::size_t collide (const CollisionObject< double > *o1, const CollisionObject< double > *o2, const CollisionRequest< double > &request, CollisionResult< double > &result)
 
template std::size_t collide (const CollisionGeometry< double > *o1, const Transform3< double > &tf1, const CollisionGeometry< double > *o2, const Transform3< double > &tf2, const CollisionRequest< double > &request, CollisionResult< double > &result)
 
template<typename GJKSolver >
detail::CollisionFunctionMatrix< GJKSolver > & getCollisionFunctionLookTable ()
 
template<typename S , typename NarrowPhaseSolver >
std::size_t collide (const CollisionObject< S > *o1, const CollisionObject< S > *o2, const NarrowPhaseSolver *nsolver, const CollisionRequest< S > &request, CollisionResult< S > &result)
 
template<typename S , typename NarrowPhaseSolver >
std::size_t collide (const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const NarrowPhaseSolver *nsolver_, const CollisionRequest< S > &request, CollisionResult< S > &result)
 
template<typename S >
std::size_t collide (const CollisionObject< S > *o1, const CollisionObject< S > *o2, const CollisionRequest< S > &request, CollisionResult< S > &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.
 
template<typename S >
std::size_t collide (const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const CollisionRequest< S > &request, CollisionResult< S > &result)
 
template bool comparePenDepth (const ContactPoint< double > &_cp1, const ContactPoint< double > &_cp2)
 
template void flipNormal (std::vector< ContactPoint< double >> &contacts)
 
template<typename S >
bool comparePenDepth (const ContactPoint< S > &_cp1, const ContactPoint< S > &_cp2)
 Return true if _cp1's penentration depth is less than _cp2's.
 
template<typename S >
void flipNormal (std::vector< ContactPoint< S >> &contacts)
 
template double continuousCollide (const CollisionGeometry< double > *o1, const MotionBase< double > *motion1, const CollisionGeometry< double > *o2, const MotionBase< double > *motion2, const ContinuousCollisionRequest< double > &request, ContinuousCollisionResult< double > &result)
 
template double continuousCollide (const CollisionGeometry< double > *o1, const Transform3< double > &tf1_beg, const Transform3< double > &tf1_end, const CollisionGeometry< double > *o2, const Transform3< double > &tf2_beg, const Transform3< double > &tf2_end, const ContinuousCollisionRequest< double > &request, ContinuousCollisionResult< double > &result)
 
template double continuousCollide (const CollisionObject< double > *o1, const Transform3< double > &tf1_end, const CollisionObject< double > *o2, const Transform3< double > &tf2_end, const ContinuousCollisionRequest< double > &request, ContinuousCollisionResult< double > &result)
 
template double collide (const ContinuousCollisionObject< double > *o1, const ContinuousCollisionObject< double > *o2, const ContinuousCollisionRequest< double > &request, ContinuousCollisionResult< double > &result)
 
template<typename GJKSolver >
detail::ConservativeAdvancementFunctionMatrix< GJKSolver > & getConservativeAdvancementFunctionLookTable ()
 
template<typename S >
MotionBasePtr< S > getMotionBase (const Transform3< S > &tf_beg, const Transform3< S > &tf_end, CCDMotionType motion_type)
 
template<typename S >
continuousCollideNaive (const CollisionGeometry< S > *o1, const MotionBase< S > *motion1, const CollisionGeometry< S > *o2, const MotionBase< S > *motion2, const ContinuousCollisionRequest< S > &request, ContinuousCollisionResult< S > &result)
 
template<typename S >
continuousCollideBVHPolynomial (const CollisionGeometry< S > *o1, const TranslationMotion< S > *motion1, const CollisionGeometry< S > *o2, const TranslationMotion< S > *motion2, const ContinuousCollisionRequest< S > &request, ContinuousCollisionResult< S > &result)
 
template<typename S >
continuousCollideConservativeAdvancement (const CollisionGeometry< S > *o1, const MotionBase< S > *motion1, const CollisionGeometry< S > *o2, const MotionBase< S > *motion2, const ContinuousCollisionRequest< S > &request, ContinuousCollisionResult< S > &result)
 
template<typename S >
continuousCollide (const CollisionGeometry< S > *o1, const MotionBase< S > *motion1, const CollisionGeometry< S > *o2, const MotionBase< S > *motion2, const ContinuousCollisionRequest< S > &request, ContinuousCollisionResult< S > &result)
 continous collision checking between two objects
 
template<typename S >
continuousCollide (const CollisionGeometry< S > *o1, const Transform3< S > &tf1_beg, const Transform3< S > &tf1_end, const CollisionGeometry< S > *o2, const Transform3< S > &tf2_beg, const Transform3< S > &tf2_end, const ContinuousCollisionRequest< S > &request, ContinuousCollisionResult< S > &result)
 
template<typename S >
continuousCollide (const CollisionObject< S > *o1, const Transform3< S > &tf1_end, const CollisionObject< S > *o2, const Transform3< S > &tf2_end, const ContinuousCollisionRequest< S > &request, ContinuousCollisionResult< S > &result)
 
template<typename S >
collide (const ContinuousCollisionObject< S > *o1, const ContinuousCollisionObject< S > *o2, const ContinuousCollisionRequest< S > &request, ContinuousCollisionResult< S > &result)
 
template double distance (const CollisionObject< double > *o1, const CollisionObject< double > *o2, const DistanceRequest< double > &request, DistanceResult< double > &result)
 
template double distance (const CollisionGeometry< double > *o1, const Transform3< double > &tf1, const CollisionGeometry< double > *o2, const Transform3< double > &tf2, const DistanceRequest< double > &request, DistanceResult< double > &result)
 
template<typename GJKSolver >
detail::DistanceFunctionMatrix< GJKSolver > & getDistanceFunctionLookTable ()
 
template<typename NarrowPhaseSolver >
NarrowPhaseSolver::S distance (const CollisionObject< typename NarrowPhaseSolver::S > *o1, const CollisionObject< typename NarrowPhaseSolver::S > *o2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename NarrowPhaseSolver::S > &request, DistanceResult< typename NarrowPhaseSolver::S > &result)
 
template<typename NarrowPhaseSolver >
NarrowPhaseSolver::S distance (const CollisionGeometry< typename NarrowPhaseSolver::S > *o1, const Transform3< typename NarrowPhaseSolver::S > &tf1, const CollisionGeometry< typename NarrowPhaseSolver::S > *o2, const Transform3< typename NarrowPhaseSolver::S > &tf2, const NarrowPhaseSolver *nsolver_, const DistanceRequest< typename NarrowPhaseSolver::S > &request, DistanceResult< typename NarrowPhaseSolver::S > &result)
 
template<typename S >
distance (const CollisionObject< S > *o1, const CollisionObject< S > *o2, const DistanceRequest< S > &request, DistanceResult< S > &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.
 
template<typename S >
distance (const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const DistanceRequest< S > &request, DistanceResult< S > &result)
 
template void constructBox (const AABB< double > &bv, Box< double > &box, Transform3< double > &tf)
 

Detailed Description

Main namespace.

collision and distance function on traversal nodes. these functions provide a higher level abstraction for collision functions provided in collision_func_matrix

Author
Jia Pan
Ioan Sucan
Jia Pan
Mark Moll
Jeongseok Lee jslee.nosp@m.02@g.nosp@m.mail..nosp@m.com

Enumeration Type Documentation

States for BVH construction empty->begun->processed ->replace_begun->processed -> ...... | |-> update_begun -> updated -> .....

Enumerator
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

BVH model type.

Enumerator
BVH_MODEL_TRIANGLES 

unknown model type

BVH_MODEL_POINTCLOUD 

triangle model

point cloud model

Error code for BVH.

Enumerator
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.

Function Documentation

template<typename S , typename DerivedA , typename DerivedB >
S fcl::distance ( const Eigen::MatrixBase< DerivedA > &  R0,
const Eigen::MatrixBase< DerivedB > &  T0,
const kIOS< S > &  b1,
const kIOS< S > &  b2,
Vector3< S > *  P = nullptr,
Vector3< S > *  Q = nullptr 
)

Approximate distance between two kIOS bounding volumes.

Todo:
P and Q is not returned, need implementation
template<typename S >
S fcl::distance ( const Transform3< S > &  tf,
const kIOS< S > &  b1,
const kIOS< S > &  b2,
Vector3< S > *  P = nullptr,
Vector3< S > *  Q = nullptr 
)

Approximate distance between two kIOS bounding volumes.

Todo:
P and Q is not returned, need implementation
template<typename S >
bool fcl::overlap ( const Transform3< S > &  tf,
const kIOS< S > &  b1,
const kIOS< S > &  b2 
)

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

Todo:
Not efficient
template<typename S , typename DerivedA , typename DerivedB >
bool fcl::overlap ( const Eigen::MatrixBase< DerivedA > &  R0,
const Eigen::MatrixBase< DerivedB > &  T0,
const kIOS< S > &  b1,
const kIOS< S > &  b2 
)

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

Todo:
Not efficient
template<typename S >
Halfspace< S > fcl::transform ( const Halfspace< S > &  a,
const Transform3< S > &  tf 
)

suppose the initial halfspace is n * x <= d after transform (R, T), x –> x' = R x + T and the new half space becomes n' * x' <= d' where n' = R * n and d' = d + n' * T

template<typename S >
Plane< S > fcl::transform ( const Plane< S > &  a,
const Transform3< S > &  tf 
)

suppose the initial halfspace is n * x <= d after transform (R, T), x –> x' = R x + T and the new half space becomes n' * x' <= d' where n' = R * n and d' = d + n' * T