coal 3.0.1
Coal, The Collision Detection Library. Previously known as HPP-FCL, fork of FCL -- The Flexible Collision Library
|
Main namespace. More...
Namespaces | |
namespace | detail |
namespace | details |
namespace | internal |
namespace | serialization |
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 | BVFitter |
The class for the default algorithm fitting a bounding volume to a set of points. More... | |
class | BVFitter< AABB > |
Specification of BVFitter for AABB bounding volume. 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 | BVFitterTpl |
The class for the default algorithm fitting a bounding volume to a set of points. 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. The front list reflects the subset of a BVTT that is traversed for that particular proximity query. 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 | BVHModelBase |
A base class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh) 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 | CachedMeshLoader |
class | Capsule |
Capsule It is \( { x~\in~\mathbb{R}^3, d(x, AB) \leq radius } \) where \( d(x, AB) \) is the distance between the point x and the capsule segment AB, with \( A = (0,0,-halfLength), B = (0,0,halfLength) \). More... | |
struct | CollisionCallBackBase |
Base callback class for collision queries. This class can be supersed by child classes to provide desired behaviors according to the application (e.g, only listing the potential CollisionObjects in collision). More... | |
struct | CollisionCallBackCollect |
Collision callback to collect collision pairs potentially in contacts. More... | |
struct | CollisionCallBackDefault |
Default collision callback to check collision between collision objects. More... | |
struct | CollisionData |
Collision data stores the collision request and the result given by collision algorithm. 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 | ComputeCollision |
This class reduces the cost of identifying the geometry pair. This is mostly useful for repeated shape-shape queries. More... | |
class | ComputeContactPatch |
This class reduces the cost of identifying the geometry pair. This is usefull for repeated shape-shape queries. More... | |
class | ComputeDistance |
struct | ComputeShapeShapeContactPatch |
Shape-shape contact patch computation. Assumes that csolver and the ContactPatchResult have already been set up by the ContactPatchRequest . More... | |
struct | ComputeShapeShapeContactPatch< Halfspace, Halfspace > |
struct | ComputeShapeShapeContactPatch< Halfspace, OtherShapeType > |
struct | ComputeShapeShapeContactPatch< Halfspace, Plane > |
struct | ComputeShapeShapeContactPatch< OtherShapeType, Halfspace > |
struct | ComputeShapeShapeContactPatch< OtherShapeType, Plane > |
struct | ComputeShapeShapeContactPatch< Plane, Halfspace > |
struct | ComputeShapeShapeContactPatch< Plane, OtherShapeType > |
struct | ComputeShapeShapeContactPatch< Plane, Plane > |
class | Cone |
Cone The base of the cone is at \( z = - halfLength \) and the top is at \( z = halfLength \). More... | |
struct | Contact |
Contact information returned by collision. More... | |
struct | ContactPatch |
This structure allows to encode contact patches. A contact patch is defined by a set of points belonging to a subset of a plane passing by p and supported by n , where n = Contact::normal and p = Contact::pos . If we denote by P this plane and by S1 and S2 the first and second shape of a collision pair, a contact patch is represented as a polytope which vertices all belong to P & S1 & S2 , where & denotes the set-intersection. Since a contact patch is a subset of a plane supported by n , it has a preferred direction. In Coal, the Contact::normal points from S1 to S2. In the same way, a contact patch points by default from S1 to S2. More... | |
struct | ContactPatchFunctionMatrix |
The contact patch matrix stores the functions for contact patches computation between different types of objects and provides a uniform call interface. More... | |
struct | ContactPatchRequest |
Request for a contact patch computation. More... | |
struct | ContactPatchResult |
Result for a contact patch computation. More... | |
struct | ContactPatchSolver |
Solver to compute contact patches, i.e. the intersection between two contact surfaces projected onto the shapes' separating plane. Otherwise said, a contact patch is simply the intersection between two support sets: the support set of shape S1 in direction n and the support set of shape S2 in direction -n , where n is the contact normal (satisfying the optimality conditions of GJK/EPA). More... | |
class | ConvexBaseTpl |
Base for convex polytope. More... | |
struct | ConvexBaseTplNeighbors |
struct | ConvexBaseTplSupportWarmStartPolytope |
class | ConvexTpl |
Convex polytope. More... | |
struct | CPUTimes |
class | Cylinder |
Cylinder along Z axis. The cylinder is defined at its centroid. More... | |
struct | DistanceCallBackBase |
Base callback class for distance queries. This class can be supersed by child classes to provide desired behaviors according to the application (e.g, only listing the potential CollisionObjects in collision). More... | |
struct | DistanceCallBackDefault |
Default distance callback to check collision between collision objects. More... | |
struct | DistanceData |
Distance data stores the distance request and the result given by distance algorithm. 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 | DynamicAABBTreeArrayCollisionManager |
class | DynamicAABBTreeCollisionManager |
class | Ellipsoid |
Ellipsoid centered at point zero. More... | |
struct | GJKSolver |
collision and distance solver based on the GJK and EPA algorithms. Originally, GJK and EPA were implemented in fcl which itself took inspiration from the code of the GJK in bullet. Since then, both GJK and EPA have been largely modified to be faster and more robust to numerical accuracy and edge cases. More... | |
class | Halfspace |
Half Space: this is equivalent to the Plane in ODE. A Half space has a priviledged direction: the direction of the normal. 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. Note: prefer using a Halfspace instead of a Plane if possible, it has better behavior w.r.t. collision detection algorithms. More... | |
class | HeightField |
Data structure depicting a height field given by the base grid dimensions and the elevation along the grid. More... | |
struct | HFNode |
struct | HFNodeBase |
class | IntervalTreeCollisionManager |
Collision manager based on interval tree. More... | |
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... | |
class | MeshLoader |
class | NaiveCollisionManager |
Brute force N-body collision manager. More... | |
struct | OBB |
Oriented bounding box class. More... | |
struct | 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 | Plane |
Infinite plane. A plane can be viewed as two half spaces; it has no priviledged direction. Note: prefer using a Halfspace instead of a Plane if possible, it has better behavior w.r.t. collision detection algorithms. More... | |
struct | QuadrilateralTpl |
Quadrilateral with 4 indices for points. More... | |
struct | QueryRequest |
base class for all query requests More... | |
struct | QueryResult |
base class for all query results More... | |
struct | RSS |
A class for rectangle sphere-swept bounding volume. More... | |
class | SaPCollisionManager |
Rigorous SAP collision manager. More... | |
struct | shape_traits |
struct | shape_traits< Box > |
struct | shape_traits< Capsule > |
struct | shape_traits< Cone > |
struct | shape_traits< ConvexBaseTpl< IndexType > > |
struct | shape_traits< Cylinder > |
struct | shape_traits< Ellipsoid > |
struct | shape_traits< Halfspace > |
struct | shape_traits< Sphere > |
struct | shape_traits< TriangleP > |
struct | shape_traits_base |
class | ShapeBase |
Base class for all basic geometric shapes. More... | |
class | SpatialHashingCollisionManager |
spatial hashing collision mananger More... | |
class | Sphere |
Center at zero point sphere. More... | |
class | SSaPCollisionManager |
Simple SAP collision manager. More... | |
struct | Timer |
This class mimics the way "boost/timer/timer.hpp" operates while using the modern std::chrono library. Importantly, this class will only have an effect for C++11 and more. More... | |
class | Transform3s |
Simple transform class used locally by InterpMotion. More... | |
struct | TraversalTraitsCollision |
struct | TraversalTraitsDistance |
class | TriangleP |
Triangle stores the points instead of only indices of points. More... | |
class | TriangleTpl |
Triangle with 3 indices for points. More... | |
Typedefs | |
template<typename S > | |
using | ContinuousCollisionCallBack = bool(*)(ContinuousCollisionObject *o1, ContinuousCollisionObject *o2, void *cdata) |
Callback for continuous collision between two objects. Return value is whether can stop now. | |
template<typename S > | |
using | ContinuousDistanceCallBack = bool(*)(ContinuousCollisionObject *o1, ContinuousCollisionObject *o2, 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< Scalar > |
typedef std::list< BVHFrontNode > | BVHFrontList |
BVH front list is a list of front nodes. | |
using | SupportSet = ContactPatch |
Structure used for internal computations. A support set and a contact patch can be represented by the same structure. In fact, a contact patch is the intersection of two support sets, one with PatchDirection::DEFAULT and one with PatchDirection::INVERTED . | |
typedef double | CoalScalar |
typedef double | Scalar |
typedef Eigen::Matrix< Scalar, 3, 1 > | Vec3s |
typedef Eigen::Matrix< Scalar, 2, 1 > | Vec2s |
typedef Eigen::Matrix< Scalar, 6, 1 > | Vec6s |
typedef Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > | VecXs |
typedef Eigen::Matrix< Scalar, 3, 3 > | Matrix3s |
typedef Eigen::Matrix< Scalar, Eigen::Dynamic, 3, Eigen::RowMajor > | MatrixX3s |
typedef Eigen::Matrix< Scalar, Eigen::Dynamic, 2, Eigen::RowMajor > | MatrixX2s |
typedef Eigen::Matrix< Eigen::DenseIndex, Eigen::Dynamic, 3, Eigen::RowMajor > | Matrixx3i |
typedef Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > | MatrixXs |
typedef Eigen::Vector2i | support_func_guess_t |
typedef double | SolverScalar |
typedef Eigen::Matrix< SolverScalar, 3, 1 > | Vec3ps |
typedef TriangleTpl< std::uint16_t > | Triangle16 |
typedef TriangleTpl< std::uint32_t > | Triangle32 |
typedef Triangle32 | Triangle |
typedef QuadrilateralTpl< std::uint16_t > | Quadrilateral16 |
typedef QuadrilateralTpl< std::uint32_t > | Quadrilateral32 |
typedef Quadrilateral32 | Quadrilateral |
typedef shared_ptr< CollisionObject > | CollisionObjectPtr_t |
typedef shared_ptr< const CollisionObject > | CollisionObjectConstPtr_t |
typedef shared_ptr< CollisionGeometry > | CollisionGeometryPtr_t |
typedef shared_ptr< const CollisionGeometry > | CollisionGeometryConstPtr_t |
typedef shared_ptr< BVHModelBase > | BVHModelPtr_t |
typedef shared_ptr< OcTree > | OcTreePtr_t |
typedef shared_ptr< const OcTree > | OcTreeConstPtr_t |
typedef Eigen::Quaternion< Scalar > | Quaternion3f |
typedef Eigen::Quaternion< Scalar > | Quats |
typedef ConvexTpl< Triangle16 > | Convex16 |
typedef ConvexTpl< Triangle32 > | Convex32 |
typedef ConvexBaseTpl< Triangle16::IndexType > | ConvexBase16 |
typedef ConvexBaseTpl< Triangle32::IndexType > | ConvexBase32 |
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 , BVH_ERR_BUILD_OUT_OF_SEQUENCE , BVH_ERR_BUILD_EMPTY_MODEL = -3 , BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME , 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 | 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_HFIELD , 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_CONVEX16 , GEOM_CONVEX32 , GEOM_CONVEX = GEOM_CONVEX32 , GEOM_PLANE , GEOM_HALFSPACE , GEOM_TRIANGLE , GEOM_OCTREE , GEOM_ELLIPSOID , HF_AABB , HF_OBBRSS , 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, triangle), and octree More... | |
enum | GJKInitialGuess { DefaultGuess , CachedGuess , BoundingVolumeGuess } |
Initial guess to use for the GJK algorithm DefaultGuess: Vec3s(1, 0, 0) CachedGuess: previous vector found by GJK or guess cached by the user BoundingVolumeGuess: guess using the centers of the shapes' AABB WARNING: to use BoundingVolumeGuess, computeLocalAABB must have been called on the two shapes. More... | |
enum | GJKVariant { DefaultGJK , PolyakAcceleration , NesterovAcceleration } |
Variant to use for the GJK algorithm. More... | |
enum | GJKConvergenceCriterion { Default , DualityGap , Hybrid } |
Which convergence criterion is used to stop the algorithm (when the shapes are not in collision). (default) VDB: Van den Bergen (A Fast and Robust GJK Implementation, 1999) DG: duality-gap, as used in the Frank-Wolfe and the vanilla 1988 GJK algorithms Hybrid: a mix between VDB and DG. More... | |
enum | GJKConvergenceCriterionType { Relative , Absolute } |
Wether the convergence criterion is scaled on the norm of the solution or not. More... | |
enum | SplitMethodType { SPLIT_METHOD_MEAN , SPLIT_METHOD_MEDIAN , SPLIT_METHOD_BV_CENTER } |
Three types of split algorithms are provided in FCL as default. More... | |
Functions | |
bool | defaultCollisionFunction (CollisionObject *o1, CollisionObject *o2, void *data) |
Provides a simple callback for the collision query in the BroadPhaseCollisionManager. It assumes the data parameter is non-null and points to an instance of CollisionData. It simply invokes the collide() method on the culled pair of geometries and stores the results in the data's CollisionResult instance. | |
bool | defaultDistanceFunction (CollisionObject *o1, CollisionObject *o2, void *data, Scalar &dist) |
Collision data for use with the DefaultContinuousCollisionFunction. It stores the collision request and the result given by the collision algorithm (and stores the conclusion of whether further evaluation of the broadphase collision manager has been deemed unnecessary). | |
bool | overlap (const Matrix3s &R0, const Vec3s &T0, const AABB &b1, const AABB &b2) |
Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity. | |
bool | overlap (const Matrix3s &R0, const Vec3s &T0, const AABB &b1, const AABB &b2, const CollisionRequest &request, Scalar &sqrDistLowerBound) |
Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity. | |
template<short N> | |
bool | overlap (const Matrix3s &, const Vec3s &, const KDOP< N > &, const KDOP< N > &) |
template<short N> | |
bool | overlap (const Matrix3s &, const Vec3s &, const KDOP< N > &, const KDOP< N > &, const CollisionRequest &, Scalar &) |
template<short N> | |
KDOP< N > | translate (const KDOP< N > &bv, const Vec3s &t) |
translate the KDOP BV | |
kIOS | translate (const kIOS &bv, const Vec3s &t) |
Translate the kIOS BV. | |
bool | overlap (const Matrix3s &R0, const Vec3s &T0, const kIOS &b1, const kIOS &b2) |
Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity. | |
bool | overlap (const Matrix3s &R0, const Vec3s &T0, const kIOS &b1, const kIOS &b2, const CollisionRequest &request, Scalar &sqrDistLowerBound) |
Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity. | |
Scalar | distance (const Matrix3s &R0, const Vec3s &T0, const kIOS &b1, const kIOS &b2, Vec3s *P=NULL, Vec3s *Q=NULL) |
Approximate distance between two kIOS bounding volumes. | |
OBB | translate (const OBB &bv, const Vec3s &t) |
Translate the OBB bv. | |
bool | overlap (const Matrix3s &R0, const Vec3s &T0, const OBB &b1, const OBB &b2) |
Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity. | |
bool | overlap (const Matrix3s &R0, const Vec3s &T0, const OBB &b1, const OBB &b2, const CollisionRequest &request, Scalar &sqrDistLowerBound) |
Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity. | |
bool | obbDisjoint (const Matrix3s &B, const Vec3s &T, const Vec3s &a, const Vec3s &b) |
bool | overlap (const Matrix3s &R0, const Vec3s &T0, const OBBRSS &b1, const OBBRSS &b2) |
Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity. | |
bool | overlap (const Matrix3s &R0, const Vec3s &T0, const OBBRSS &b1, const OBBRSS &b2, const CollisionRequest &request, Scalar &sqrDistLowerBound) |
Scalar | distance (const Matrix3s &R0, const Vec3s &T0, const OBBRSS &b1, const OBBRSS &b2, Vec3s *P=NULL, Vec3s *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. | |
Scalar | distance (const Matrix3s &R0, const Vec3s &T0, const RSS &b1, const RSS &b2, Vec3s *P=NULL, Vec3s *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) | |
bool | overlap (const Matrix3s &R0, const Vec3s &T0, const RSS &b1, const RSS &b2) |
Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity. | |
bool | overlap (const Matrix3s &R0, const Vec3s &T0, const RSS &b1, const RSS &b2, const CollisionRequest &request, Scalar &sqrDistLowerBound) |
Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity. | |
void | updateFrontList (BVHFrontList *front_list, unsigned int b1, unsigned int b2) |
Add new front node into the front list. | |
template<typename BV > | |
BVHModel< BV > * | BVHExtract (const BVHModel< BV > &model, const Transform3s &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. | |
template<> | |
BVHModel< OBB > * | BVHExtract (const BVHModel< OBB > &model, const Transform3s &pose, const AABB &aabb) |
template<> | |
BVHModel< AABB > * | BVHExtract (const BVHModel< AABB > &model, const Transform3s &pose, const AABB &aabb) |
template<> | |
BVHModel< RSS > * | BVHExtract (const BVHModel< RSS > &model, const Transform3s &pose, const AABB &aabb) |
template<> | |
BVHModel< kIOS > * | BVHExtract (const BVHModel< kIOS > &model, const Transform3s &pose, const AABB &aabb) |
template<> | |
BVHModel< OBBRSS > * | BVHExtract (const BVHModel< OBBRSS > &model, const Transform3s &pose, const AABB &aabb) |
template<> | |
BVHModel< KDOP< 16 > > * | BVHExtract (const BVHModel< KDOP< 16 > > &model, const Transform3s &pose, const AABB &aabb) |
template<> | |
BVHModel< KDOP< 18 > > * | BVHExtract (const BVHModel< KDOP< 18 > > &model, const Transform3s &pose, const AABB &aabb) |
template<> | |
BVHModel< KDOP< 24 > > * | BVHExtract (const BVHModel< KDOP< 24 > > &model, const Transform3s &pose, const AABB &aabb) |
void | getCovariance (Vec3s *ps, Vec3s *ps2, Triangle32 *ts, unsigned int *indices, unsigned int n, Matrix3s &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. | |
void | getRadiusAndOriginAndRectangleSize (Vec3s *ps, Vec3s *ps2, Triangle32 *ts, unsigned int *indices, unsigned int n, const Matrix3s &axes, Vec3s &origin, Scalar l[2], Scalar &r) |
Compute the RSS bounding volume parameters: radius, rectangle size and the origin, given the BV axises. | |
void | getExtentAndCenter (Vec3s *ps, Vec3s *ps2, Triangle32 *ts, unsigned int *indices, unsigned int n, Matrix3s &axes, Vec3s ¢er, Vec3s &extent) |
Compute the bounding volume extent and center for a set or subset of points, given the BV axises. | |
void | circumCircleComputation (const Vec3s &a, const Vec3s &b, const Vec3s &c, Vec3s ¢er, Scalar &radius) |
Compute the center and radius for a triangle's circumcircle. | |
Scalar | maximumDistance (Vec3s *ps, Vec3s *ps2, Triangle32 *ts, unsigned int *indices, unsigned int n, const Vec3s &query) |
Compute the maximum distance from a given center point to a point cloud. | |
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. Return value is the number of contacts generated between the two objects. | |
std::size_t | collide (const CollisionGeometry *o1, const Transform3s &tf1, const CollisionGeometry *o2, const Transform3s &tf2, const CollisionRequest &request, CollisionResult &result) |
void | constructContactPatchFrameFromContact (const Contact &contact, ContactPatch &contact_patch) |
Construct a frame from a Contact 's position and normal. Because both Contact 's position and normal are expressed in the world frame, this frame is also expressed w.r.t the world frame. The origin of the frame is contact.pos and the z-axis of the frame is contact.normal . | |
CollisionRequestFlag | operator~ (CollisionRequestFlag a) |
CollisionRequestFlag | operator| (CollisionRequestFlag a, CollisionRequestFlag b) |
CollisionRequestFlag | operator& (CollisionRequestFlag a, CollisionRequestFlag b) |
CollisionRequestFlag | operator^ (CollisionRequestFlag a, CollisionRequestFlag b) |
CollisionRequestFlag & | operator|= (CollisionRequestFlag &a, CollisionRequestFlag b) |
CollisionRequestFlag & | operator&= (CollisionRequestFlag &a, CollisionRequestFlag b) |
CollisionRequestFlag & | operator^= (CollisionRequestFlag &a, CollisionRequestFlag b) |
CollisionGeometry * | extract (const CollisionGeometry *model, const Transform3s &pose, const AABB &aabb) |
const char * | get_node_type_name (NODE_TYPE node_type) |
Returns the name associated to a NODE_TYPE. | |
const char * | get_object_type_name (OBJECT_TYPE object_type) |
Returns the name associated to a OBJECT_TYPE. | |
void | computeContactPatch (const CollisionGeometry *o1, const Transform3s &tf1, const CollisionGeometry *o2, const Transform3s &tf2, const CollisionResult &collision_result, const ContactPatchRequest &request, ContactPatchResult &result) |
Main contact patch computation interface. | |
void | computeContactPatch (const CollisionObject *o1, const CollisionObject *o2, const CollisionResult &collision_result, const ContactPatchRequest &request, ContactPatchResult &result) |
Scalar | 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. | |
Scalar | distance (const CollisionGeometry *o1, const Transform3s &tf1, const CollisionGeometry *o2, const Transform3s &tf2, const DistanceRequest &request, DistanceResult &result) |
HFNodeBase::FaceOrientation | operator& (HFNodeBase::FaceOrientation a, HFNodeBase::FaceOrientation b) |
int | operator& (int a, HFNodeBase::FaceOrientation b) |
template<typename BV > | |
void | fit (Vec3s *ps, unsigned int n, BV &bv) |
Compute a bounding volume that fits a set of n points. | |
template<> | |
void | fit< OBB > (Vec3s *ps, unsigned int n, OBB &bv) |
template<> | |
void | fit< RSS > (Vec3s *ps, unsigned int n, RSS &bv) |
template<> | |
void | fit< kIOS > (Vec3s *ps, unsigned int n, kIOS &bv) |
template<> | |
void | fit< OBBRSS > (Vec3s *ps, unsigned int n, OBBRSS &bv) |
template<> | |
void | fit< AABB > (Vec3s *ps, unsigned int n, AABB &bv) |
template<bool InvertShapes, typename OtherShapeType , typename PlaneOrHalfspace > | |
void | computePatchPlaneOrHalfspace (const OtherShapeType &s1, const Transform3s &tf1, const PlaneOrHalfspace &s2, const Transform3s &tf2, const ContactPatchSolver *csolver, const Contact &contact, ContactPatch &contact_patch) |
Computes the contact patch between a Plane/Halfspace and another shape. | |
template<typename ShapeType1 , typename ShapeType2 > | |
void | ShapeShapeContactPatch (const CollisionGeometry *o1, const Transform3s &tf1, const CollisionGeometry *o2, const Transform3s &tf2, const CollisionResult &collision_result, const ContactPatchSolver *csolver, const ContactPatchRequest &request, ContactPatchResult &result) |
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 | 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 | |
template<typename Derived , typename OtherDerived > | |
bool | isEqual (const Eigen::MatrixBase< Derived > &lhs, const Eigen::MatrixBase< OtherDerived > &rhs, const Scalar tol=std::numeric_limits< Scalar >::epsilon() *100) |
template<typename Derived > | |
Quats | fromAxisAngle (const Eigen::MatrixBase< Derived > &axis, Scalar angle) |
Quats | uniformRandomQuaternion () |
Uniformly random quaternion sphere. Code taken from Pinocchio (https://github.com/stack-of-tasks/pinocchio). | |
Matrix3s | constructOrthonormalBasisFromVector (const Vec3s &vec) |
Construct othonormal basis from vector. The z-axis is the normalized input vector. | |
template<class BoundingVolume > | |
void | loadPolyhedronFromResource (const std::string &resource_path, const coal::Vec3s &scale, const shared_ptr< BVHModel< BoundingVolume > > &polyhedron) |
Read a mesh file and convert it to a polyhedral mesh. | |
OcTreePtr_t | makeOctree (const Eigen::Matrix< Scalar, Eigen::Dynamic, 3 > &point_cloud, const Scalar resolution) |
Build an OcTree from a point cloud and a given resolution. | |
template<typename T > | |
size_t | computeMemoryFootprint (const T &object) |
Returns the memory footpring of the input object. For POD objects, this function returns the result of sizeof(T) | |
template<typename BV > | |
void | generateBVHModel (BVHModel< BV > &model, const Box &shape, const Transform3s &pose) |
Generate BVH model from box. | |
template<typename BV > | |
void | generateBVHModel (BVHModel< BV > &model, const Sphere &shape, const Transform3s &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 &shape, const Transform3s &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 Cylinder &shape, const Transform3s &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 &shape, const Transform3s &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 &shape, const Transform3s &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 &shape, const Transform3s &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<typename BV , typename S > | |
void | computeBV (const S &s, const Transform3s &tf, BV &bv) |
calculate a bounding volume for a shape in a specific configuration | |
template<typename IndexType > | |
void | reorderTriangle (const ConvexTpl< TriangleTpl< IndexType > > *convex_tri, TriangleTpl< IndexType > &tri) |
template<> | |
void | computeBV< AABB, Box > (const Box &s, const Transform3s &tf, AABB &bv) |
template<> | |
void | computeBV< AABB, Sphere > (const Sphere &s, const Transform3s &tf, AABB &bv) |
template<> | |
void | computeBV< AABB, Ellipsoid > (const Ellipsoid &e, const Transform3s &tf, AABB &bv) |
template<> | |
void | computeBV< AABB, Capsule > (const Capsule &s, const Transform3s &tf, AABB &bv) |
template<> | |
void | computeBV< AABB, Cone > (const Cone &s, const Transform3s &tf, AABB &bv) |
template<> | |
void | computeBV< AABB, Cylinder > (const Cylinder &s, const Transform3s &tf, AABB &bv) |
template<> | |
void | computeBV< AABB, ConvexBase32 > (const ConvexBase32 &s, const Transform3s &tf, AABB &bv) |
template<> | |
void | computeBV< AABB, ConvexBase16 > (const ConvexBase16 &s, const Transform3s &tf, AABB &bv) |
template<> | |
void | computeBV< AABB, TriangleP > (const TriangleP &s, const Transform3s &tf, AABB &bv) |
template<> | |
void | computeBV< AABB, Halfspace > (const Halfspace &s, const Transform3s &tf, AABB &bv) |
template<> | |
void | computeBV< AABB, Plane > (const Plane &s, const Transform3s &tf, AABB &bv) |
template<> | |
void | computeBV< OBB, Box > (const Box &s, const Transform3s &tf, OBB &bv) |
template<> | |
void | computeBV< OBB, Sphere > (const Sphere &s, const Transform3s &tf, OBB &bv) |
template<> | |
void | computeBV< OBB, Capsule > (const Capsule &s, const Transform3s &tf, OBB &bv) |
template<> | |
void | computeBV< OBB, Cone > (const Cone &s, const Transform3s &tf, OBB &bv) |
template<> | |
void | computeBV< OBB, Cylinder > (const Cylinder &s, const Transform3s &tf, OBB &bv) |
template<> | |
void | computeBV< OBB, ConvexBase32 > (const ConvexBase32 &s, const Transform3s &tf, OBB &bv) |
template<> | |
void | computeBV< OBB, ConvexBase16 > (const ConvexBase16 &s, const Transform3s &tf, OBB &bv) |
template<> | |
void | computeBV< OBB, Halfspace > (const Halfspace &s, const Transform3s &tf, OBB &bv) |
template<> | |
void | computeBV< RSS, Halfspace > (const Halfspace &s, const Transform3s &tf, RSS &bv) |
template<> | |
void | computeBV< OBBRSS, Halfspace > (const Halfspace &s, const Transform3s &tf, OBBRSS &bv) |
template<> | |
void | computeBV< kIOS, Halfspace > (const Halfspace &s, const Transform3s &tf, kIOS &bv) |
template<> | |
void | computeBV< KDOP< 16 >, Halfspace > (const Halfspace &s, const Transform3s &tf, KDOP< 16 > &bv) |
template<> | |
void | computeBV< KDOP< 18 >, Halfspace > (const Halfspace &s, const Transform3s &tf, KDOP< 18 > &bv) |
template<> | |
void | computeBV< KDOP< 24 >, Halfspace > (const Halfspace &s, const Transform3s &tf, KDOP< 24 > &bv) |
template<> | |
void | computeBV< OBB, Plane > (const Plane &s, const Transform3s &tf, OBB &bv) |
template<> | |
void | computeBV< RSS, Plane > (const Plane &s, const Transform3s &tf, RSS &bv) |
template<> | |
void | computeBV< OBBRSS, Plane > (const Plane &s, const Transform3s &tf, OBBRSS &bv) |
template<> | |
void | computeBV< kIOS, Plane > (const Plane &s, const Transform3s &tf, kIOS &bv) |
template<> | |
void | computeBV< KDOP< 16 >, Plane > (const Plane &s, const Transform3s &tf, KDOP< 16 > &bv) |
template<> | |
void | computeBV< KDOP< 18 >, Plane > (const Plane &s, const Transform3s &tf, KDOP< 18 > &bv) |
template<> | |
void | computeBV< KDOP< 24 >, Plane > (const Plane &s, const Transform3s &tf, KDOP< 24 > &bv) |
void | constructBox (const AABB &bv, Box &box, Transform3s &tf) |
construct a box shape (with a configuration) from a given bounding volume | |
void | constructBox (const OBB &bv, Box &box, Transform3s &tf) |
void | constructBox (const OBBRSS &bv, Box &box, Transform3s &tf) |
void | constructBox (const kIOS &bv, Box &box, Transform3s &tf) |
void | constructBox (const RSS &bv, Box &box, Transform3s &tf) |
void | constructBox (const KDOP< 16 > &bv, Box &box, Transform3s &tf) |
void | constructBox (const KDOP< 18 > &bv, Box &box, Transform3s &tf) |
void | constructBox (const KDOP< 24 > &bv, Box &box, Transform3s &tf) |
void | constructBox (const AABB &bv, const Transform3s &tf_bv, Box &box, Transform3s &tf) |
void | constructBox (const OBB &bv, const Transform3s &tf_bv, Box &box, Transform3s &tf) |
void | constructBox (const OBBRSS &bv, const Transform3s &tf_bv, Box &box, Transform3s &tf) |
void | constructBox (const kIOS &bv, const Transform3s &tf_bv, Box &box, Transform3s &tf) |
void | constructBox (const RSS &bv, const Transform3s &tf_bv, Box &box, Transform3s &tf) |
void | constructBox (const KDOP< 16 > &bv, const Transform3s &tf_bv, Box &box, Transform3s &tf) |
void | constructBox (const KDOP< 18 > &bv, const Transform3s &tf_bv, Box &box, Transform3s &tf) |
void | constructBox (const KDOP< 24 > &bv, const Transform3s &tf_bv, Box &box, Transform3s &tf) |
Halfspace | transform (const Halfspace &a, const Transform3s &tf) |
Plane | transform (const Plane &a, const Transform3s &tf) |
std::array< Halfspace, 2 > | transformToHalfspaces (const Plane &a, const Transform3s &tf) |
bool | obbDisjointAndLowerBoundDistance (const Matrix3s &B, const Vec3s &T, const Vec3s &a, const Vec3s &b, const CollisionRequest &request, Scalar &squaredLowerBoundDistance) |
Variables | |
constexpr size_t | GJK_DEFAULT_MAX_ITERATIONS = 128 |
GJK. | |
constexpr Scalar | GJK_DEFAULT_TOLERANCE = Scalar(1e-6) |
constexpr Scalar | GJK_MINIMUM_TOLERANCE = Scalar(1e-6) |
constexpr size_t | EPA_DEFAULT_MAX_ITERATIONS = 64 |
constexpr Scalar | EPA_DEFAULT_TOLERANCE = Scalar(1e-6) |
constexpr Scalar | EPA_MINIMUM_TOLERANCE = Scalar(1e-6) |
Main namespace.
This file defines different macros used to characterize the default behavior of the narrowphase algorithms GJK and EPA.
using coal::BroadPhaseContinuousCollisionManagerd = typedef BroadPhaseContinuousCollisionManager<Scalar> |
using coal::BroadPhaseContinuousCollisionManagerf = typedef BroadPhaseContinuousCollisionManager<float> |
typedef std::list<BVHFrontNode> coal::BVHFrontList |
BVH front list is a list of front nodes.
typedef shared_ptr<BVHModelBase> coal::BVHModelPtr_t |
typedef double coal::CoalScalar |
typedef shared_ptr<const CollisionGeometry> coal::CollisionGeometryConstPtr_t |
typedef shared_ptr<CollisionGeometry> coal::CollisionGeometryPtr_t |
typedef shared_ptr<const CollisionObject> coal::CollisionObjectConstPtr_t |
typedef shared_ptr<CollisionObject> coal::CollisionObjectPtr_t |
using coal::ContinuousCollisionCallBack = typedef bool (*)(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata) |
Callback for continuous collision between two objects. Return value is whether can stop now.
using coal::ContinuousDistanceCallBack = typedef bool (*)(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, S& dist) |
Callback for continuous distance between two objects, Return value is whether can stop now, also return the minimum distance till now.
typedef ConvexTpl<Triangle16> coal::Convex16 |
typedef ConvexTpl<Triangle32> coal::Convex32 |
typedef Eigen::Matrix<Scalar, 3, 3> coal::Matrix3s |
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 2, Eigen::RowMajor> coal::MatrixX2s |
typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 3, Eigen::RowMajor> coal::Matrixx3i |
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 3, Eigen::RowMajor> coal::MatrixX3s |
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> coal::MatrixXs |
typedef shared_ptr<const OcTree> coal::OcTreeConstPtr_t |
typedef shared_ptr<OcTree> coal::OcTreePtr_t |
typedef Quadrilateral32 coal::Quadrilateral |
typedef QuadrilateralTpl<std::uint16_t> coal::Quadrilateral16 |
typedef QuadrilateralTpl<std::uint32_t> coal::Quadrilateral32 |
typedef Eigen::Quaternion<Scalar> coal::Quaternion3f |
typedef Eigen::Quaternion<Scalar> coal::Quats |
typedef double coal::Scalar |
typedef double coal::SolverScalar |
typedef Eigen::Vector2i coal::support_func_guess_t |
using coal::SupportSet = typedef ContactPatch |
Structure used for internal computations. A support set and a contact patch can be represented by the same structure. In fact, a contact patch is the intersection of two support sets, one with PatchDirection::DEFAULT
and one with PatchDirection::INVERTED
.
DEFAULT
direction is the support set of a shape in the direction given by +n
, where n
is the z-axis of the frame's patch rotation. An INVERTED
support set is the support set of a shape in the direction -n
. typedef Triangle32 coal::Triangle |
typedef TriangleTpl<std::uint16_t> coal::Triangle16 |
typedef TriangleTpl<std::uint32_t> coal::Triangle32 |
typedef Eigen::Matrix<Scalar, 2, 1> coal::Vec2s |
typedef Eigen::Matrix<SolverScalar, 3, 1> coal::Vec3ps |
typedef Eigen::Matrix<Scalar, 3, 1> coal::Vec3s |
typedef Eigen::Matrix<Scalar, 6, 1> coal::Vec6s |
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> coal::VecXs |
enum coal::BVHBuildState |
States for BVH construction empty->begun->processed ->replace_begun->processed -> ...... | |-> update_begun -> updated -> .....
enum coal::BVHModelType |
enum coal::BVHReturnCode |
Error code for BVH.
flag declaration for specifying required params in CollisionResult
Enumerator | |
---|---|
CONTACT | |
DISTANCE_LOWER_BOUND | |
NO_REQUEST |
Which convergence criterion is used to stop the algorithm (when the shapes are not in collision). (default) VDB: Van den Bergen (A Fast and Robust GJK Implementation, 1999) DG: duality-gap, as used in the Frank-Wolfe and the vanilla 1988 GJK algorithms Hybrid: a mix between VDB and DG.
Enumerator | |
---|---|
Default | |
DualityGap | |
Hybrid |
Initial guess to use for the GJK algorithm DefaultGuess: Vec3s(1, 0, 0) CachedGuess: previous vector found by GJK or guess cached by the user BoundingVolumeGuess: guess using the centers of the shapes' AABB WARNING: to use BoundingVolumeGuess, computeLocalAABB must have been called on the two shapes.
Enumerator | |
---|---|
DefaultGuess | |
CachedGuess | |
BoundingVolumeGuess |
enum coal::GJKVariant |
enum coal::NODE_TYPE |
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, cylinder, convex, plane, triangle), and octree
enum coal::OBJECT_TYPE |
BVHModel< AABB > * coal::BVHExtract | ( | const BVHModel< AABB > & | model, |
const Transform3s & | pose, | ||
const AABB & | aabb | ||
) |
BVHModel< BV > * coal::BVHExtract | ( | const BVHModel< BV > & | model, |
const Transform3s & | pose, | ||
const AABB & | aabb | ||
) |
BVHModel< KDOP< 16 > > * coal::BVHExtract | ( | const BVHModel< KDOP< 16 > > & | model, |
const Transform3s & | pose, | ||
const AABB & | aabb | ||
) |
BVHModel< KDOP< 18 > > * coal::BVHExtract | ( | const BVHModel< KDOP< 18 > > & | model, |
const Transform3s & | pose, | ||
const AABB & | aabb | ||
) |
BVHModel< KDOP< 24 > > * coal::BVHExtract | ( | const BVHModel< KDOP< 24 > > & | model, |
const Transform3s & | pose, | ||
const AABB & | aabb | ||
) |
BVHModel< kIOS > * coal::BVHExtract | ( | const BVHModel< kIOS > & | model, |
const Transform3s & | pose, | ||
const AABB & | aabb | ||
) |
BVHModel< OBB > * coal::BVHExtract | ( | const BVHModel< OBB > & | model, |
const Transform3s & | pose, | ||
const AABB & | aabb | ||
) |
BVHModel< OBBRSS > * coal::BVHExtract | ( | const BVHModel< OBBRSS > & | model, |
const Transform3s & | pose, | ||
const AABB & | aabb | ||
) |
BVHModel< RSS > * coal::BVHExtract | ( | const BVHModel< RSS > & | model, |
const Transform3s & | pose, | ||
const AABB & | aabb | ||
) |
void coal::circumCircleComputation | ( | const Vec3s & | a, |
const Vec3s & | b, | ||
const Vec3s & | c, | ||
Vec3s & | center, | ||
Scalar & | radius | ||
) |
Compute the center and radius for a triangle's circumcircle.
std::size_t coal::collide | ( | const CollisionGeometry * | o1, |
const Transform3s & | tf1, | ||
const CollisionGeometry * | o2, | ||
const Transform3s & | tf2, | ||
const CollisionRequest & | request, | ||
CollisionResult & | result | ||
) |
std::size_t coal::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.
|
inline |
calculate a bounding volume for a shape in a specific configuration
void coal::computeBV< AABB, Box > | ( | const Box & | s, |
const Transform3s & | tf, | ||
AABB & | bv | ||
) |
void coal::computeBV< AABB, Capsule > | ( | const Capsule & | s, |
const Transform3s & | tf, | ||
AABB & | bv | ||
) |
void coal::computeBV< AABB, Cone > | ( | const Cone & | s, |
const Transform3s & | tf, | ||
AABB & | bv | ||
) |
void coal::computeBV< AABB, ConvexBase16 > | ( | const ConvexBase16 & | s, |
const Transform3s & | tf, | ||
AABB & | bv | ||
) |
void coal::computeBV< AABB, ConvexBase32 > | ( | const ConvexBase32 & | s, |
const Transform3s & | tf, | ||
AABB & | bv | ||
) |
void coal::computeBV< AABB, Cylinder > | ( | const Cylinder & | s, |
const Transform3s & | tf, | ||
AABB & | bv | ||
) |
void coal::computeBV< AABB, Ellipsoid > | ( | const Ellipsoid & | e, |
const Transform3s & | tf, | ||
AABB & | bv | ||
) |
void coal::computeBV< AABB, Halfspace > | ( | const Halfspace & | s, |
const Transform3s & | tf, | ||
AABB & | bv | ||
) |
void coal::computeBV< AABB, Plane > | ( | const Plane & | s, |
const Transform3s & | tf, | ||
AABB & | bv | ||
) |
void coal::computeBV< AABB, Sphere > | ( | const Sphere & | s, |
const Transform3s & | tf, | ||
AABB & | bv | ||
) |
void coal::computeBV< AABB, TriangleP > | ( | const TriangleP & | s, |
const Transform3s & | tf, | ||
AABB & | bv | ||
) |
void coal::computeBV< KDOP< 16 >, Halfspace > | ( | const Halfspace & | s, |
const Transform3s & | tf, | ||
KDOP< 16 > & | bv | ||
) |
void coal::computeBV< KDOP< 16 >, Plane > | ( | const Plane & | s, |
const Transform3s & | tf, | ||
KDOP< 16 > & | bv | ||
) |
void coal::computeBV< KDOP< 18 >, Halfspace > | ( | const Halfspace & | s, |
const Transform3s & | tf, | ||
KDOP< 18 > & | bv | ||
) |
void coal::computeBV< KDOP< 18 >, Plane > | ( | const Plane & | s, |
const Transform3s & | tf, | ||
KDOP< 18 > & | bv | ||
) |
void coal::computeBV< KDOP< 24 >, Halfspace > | ( | const Halfspace & | s, |
const Transform3s & | tf, | ||
KDOP< 24 > & | bv | ||
) |
void coal::computeBV< KDOP< 24 >, Plane > | ( | const Plane & | s, |
const Transform3s & | tf, | ||
KDOP< 24 > & | bv | ||
) |
void coal::computeBV< kIOS, Halfspace > | ( | const Halfspace & | s, |
const Transform3s & | tf, | ||
kIOS & | bv | ||
) |
void coal::computeBV< kIOS, Plane > | ( | const Plane & | s, |
const Transform3s & | tf, | ||
kIOS & | bv | ||
) |
void coal::computeBV< OBB, Box > | ( | const Box & | s, |
const Transform3s & | tf, | ||
OBB & | bv | ||
) |
void coal::computeBV< OBB, Capsule > | ( | const Capsule & | s, |
const Transform3s & | tf, | ||
OBB & | bv | ||
) |
void coal::computeBV< OBB, Cone > | ( | const Cone & | s, |
const Transform3s & | tf, | ||
OBB & | bv | ||
) |
void coal::computeBV< OBB, ConvexBase16 > | ( | const ConvexBase16 & | s, |
const Transform3s & | tf, | ||
OBB & | bv | ||
) |
void coal::computeBV< OBB, ConvexBase32 > | ( | const ConvexBase32 & | s, |
const Transform3s & | tf, | ||
OBB & | bv | ||
) |
void coal::computeBV< OBB, Cylinder > | ( | const Cylinder & | s, |
const Transform3s & | tf, | ||
OBB & | bv | ||
) |
void coal::computeBV< OBB, Halfspace > | ( | const Halfspace & | s, |
const Transform3s & | tf, | ||
OBB & | bv | ||
) |
void coal::computeBV< OBB, Plane > | ( | const Plane & | s, |
const Transform3s & | tf, | ||
OBB & | bv | ||
) |
void coal::computeBV< OBB, Sphere > | ( | const Sphere & | s, |
const Transform3s & | tf, | ||
OBB & | bv | ||
) |
void coal::computeBV< OBBRSS, Halfspace > | ( | const Halfspace & | s, |
const Transform3s & | tf, | ||
OBBRSS & | bv | ||
) |
void coal::computeBV< OBBRSS, Plane > | ( | const Plane & | s, |
const Transform3s & | tf, | ||
OBBRSS & | bv | ||
) |
void coal::computeBV< RSS, Halfspace > | ( | const Halfspace & | s, |
const Transform3s & | tf, | ||
RSS & | bv | ||
) |
void coal::computeBV< RSS, Plane > | ( | const Plane & | s, |
const Transform3s & | tf, | ||
RSS & | bv | ||
) |
void coal::computeContactPatch | ( | const CollisionGeometry * | o1, |
const Transform3s & | tf1, | ||
const CollisionGeometry * | o2, | ||
const Transform3s & | tf2, | ||
const CollisionResult & | collision_result, | ||
const ContactPatchRequest & | request, | ||
ContactPatchResult & | result | ||
) |
Main contact patch computation interface.
void coal::computeContactPatch | ( | const CollisionObject * | o1, |
const CollisionObject * | o2, | ||
const CollisionResult & | collision_result, | ||
const ContactPatchRequest & | request, | ||
ContactPatchResult & | result | ||
) |
size_t coal::computeMemoryFootprint | ( | const T & | object | ) |
Returns the memory footpring of the input object. For POD objects, this function returns the result of sizeof(T)
[in] | object | whose memory footprint needs to be evaluated. |
void coal::computePatchPlaneOrHalfspace | ( | const OtherShapeType & | s1, |
const Transform3s & | tf1, | ||
const PlaneOrHalfspace & | s2, | ||
const Transform3s & | tf2, | ||
const ContactPatchSolver * | csolver, | ||
const Contact & | contact, | ||
ContactPatch & | contact_patch | ||
) |
Computes the contact patch between a Plane/Halfspace and another shape.
InvertShapes | set to true if the first shape of the collision pair is s2 and not s1 (if you had to invert (s1, tf1) and (s2, tf2) when calling this function). |
void coal::constructBox | ( | const AABB & | bv, |
Box & | box, | ||
Transform3s & | tf | ||
) |
construct a box shape (with a configuration) from a given bounding volume
void coal::constructBox | ( | const AABB & | bv, |
const Transform3s & | tf_bv, | ||
Box & | box, | ||
Transform3s & | tf | ||
) |
void coal::constructBox | ( | const KDOP< 16 > & | bv, |
Box & | box, | ||
Transform3s & | tf | ||
) |
void coal::constructBox | ( | const KDOP< 16 > & | bv, |
const Transform3s & | tf_bv, | ||
Box & | box, | ||
Transform3s & | tf | ||
) |
void coal::constructBox | ( | const KDOP< 18 > & | bv, |
Box & | box, | ||
Transform3s & | tf | ||
) |
void coal::constructBox | ( | const KDOP< 18 > & | bv, |
const Transform3s & | tf_bv, | ||
Box & | box, | ||
Transform3s & | tf | ||
) |
void coal::constructBox | ( | const KDOP< 24 > & | bv, |
Box & | box, | ||
Transform3s & | tf | ||
) |
void coal::constructBox | ( | const KDOP< 24 > & | bv, |
const Transform3s & | tf_bv, | ||
Box & | box, | ||
Transform3s & | tf | ||
) |
void coal::constructBox | ( | const kIOS & | bv, |
Box & | box, | ||
Transform3s & | tf | ||
) |
void coal::constructBox | ( | const kIOS & | bv, |
const Transform3s & | tf_bv, | ||
Box & | box, | ||
Transform3s & | tf | ||
) |
void coal::constructBox | ( | const OBB & | bv, |
Box & | box, | ||
Transform3s & | tf | ||
) |
void coal::constructBox | ( | const OBB & | bv, |
const Transform3s & | tf_bv, | ||
Box & | box, | ||
Transform3s & | tf | ||
) |
void coal::constructBox | ( | const OBBRSS & | bv, |
Box & | box, | ||
Transform3s & | tf | ||
) |
void coal::constructBox | ( | const OBBRSS & | bv, |
const Transform3s & | tf_bv, | ||
Box & | box, | ||
Transform3s & | tf | ||
) |
void coal::constructBox | ( | const RSS & | bv, |
Box & | box, | ||
Transform3s & | tf | ||
) |
void coal::constructBox | ( | const RSS & | bv, |
const Transform3s & | tf_bv, | ||
Box & | box, | ||
Transform3s & | tf | ||
) |
|
inline |
Construct othonormal basis from vector. The z-axis is the normalized input vector.
bool coal::defaultCollisionFunction | ( | CollisionObject * | o1, |
CollisionObject * | o2, | ||
void * | data | ||
) |
Provides a simple callback for the collision query in the BroadPhaseCollisionManager. It assumes the data
parameter is non-null and points to an instance of CollisionData. It simply invokes the collide()
method on the culled pair of geometries and stores the results in the data's CollisionResult instance.
This callback will cause the broadphase evaluation to stop if:
For a given instance of CollisionData, if broadphase evaluation has already terminated (i.e., defaultCollisionFunction() returned true
), subsequent invocations with the same instance of CollisionData will return immediately, requesting termination of broadphase evaluation (i.e., return true
).
o1 | The first object in the culled pair. |
o2 | The second object in the culled pair. |
data | A non-null pointer to a CollisionData instance. |
true
if the broadphase evaluation should stop. S | The scalar type with which the computation will be performed. |
bool coal::defaultDistanceFunction | ( | CollisionObject * | o1, |
CollisionObject * | o2, | ||
void * | data, | ||
Scalar & | dist | ||
) |
Collision data for use with the DefaultContinuousCollisionFunction. It stores the collision request and the result given by the collision algorithm (and stores the conclusion of whether further evaluation of the broadphase collision manager has been deemed unnecessary).
Provides a simple callback for the continuous collision query in the BroadPhaseCollisionManager. It assumes the data
parameter is non-null and points to an instance of DefaultContinuousCollisionData. It simply invokes the collide()
method on the culled pair of geometries and stores the results in the data's ContinuousCollisionResult instance.
This callback will never cause the broadphase evaluation to terminate early. However, if the done
member of the DefaultContinuousCollisionData is set to true, this method will simply return without doing any computation.
For a given instance of DefaultContinuousCollisionData, if broadphase evaluation has already terminated (i.e., DefaultContinuousCollisionFunction() returned true
), subsequent invocations with the same instance of CollisionData will return immediately, requesting termination of broadphase evaluation (i.e., return true
).
o1 | The first object in the culled pair. |
o2 | The second object in the culled pair. |
data | A non-null pointer to a CollisionData instance. |
S | The scalar type with which the computation will be performed. |
Provides a simple callback for the distance query in the BroadPhaseCollisionManager. It assumes the data
parameter is non-null and points to an instance of DistanceData. It simply invokes the distance()
method on the culled pair of geometries and stores the results in the data's DistanceResult instance.
This callback will cause the broadphase evaluation to stop if:
For a given instance of DistanceData, if broadphase evaluation has already terminated (i.e., defaultDistanceFunction() returned true
), subsequent invocations with the same instance of DistanceData will simply report the previously reported minimum distance and request termination of broadphase evaluation (i.e., return true
).
o1 | The first object in the culled pair. |
o2 | The second object in the culled pair. |
data | A non-null pointer to a DistanceData instance. |
dist | The distance computed by distance(). |
true
if the broadphase evaluation should stop. S | The scalar type with which the computation will be performed. |
Scalar coal::distance | ( | const CollisionGeometry * | o1, |
const Transform3s & | tf1, | ||
const CollisionGeometry * | o2, | ||
const Transform3s & | tf2, | ||
const DistanceRequest & | request, | ||
DistanceResult & | result | ||
) |
Scalar coal::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.
|
inline |
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.
Scalar coal::distance | ( | const Matrix3s & | R0, |
const Vec3s & | T0, | ||
const RSS & | b1, | ||
const RSS & | b2, | ||
Vec3s * | P = NULL , |
||
Vec3s * | 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)
void coal::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
CollisionGeometry * coal::extract | ( | const CollisionGeometry * | model, |
const Transform3s & | pose, | ||
const AABB & | aabb | ||
) |
void coal::fit | ( | Vec3s * | ps, |
unsigned int | n, | ||
BV & | bv | ||
) |
Compute a bounding volume that fits a set of n points.
|
inline |
void coal::generateBVHModel | ( | BVHModel< BV > & | model, |
const Box & | shape, | ||
const Transform3s & | pose | ||
) |
Generate BVH model from box.
void coal::generateBVHModel | ( | BVHModel< BV > & | model, |
const Cone & | shape, | ||
const Transform3s & | 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.
void coal::generateBVHModel | ( | BVHModel< BV > & | model, |
const Cone & | shape, | ||
const Transform3s & | 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.
void coal::generateBVHModel | ( | BVHModel< BV > & | model, |
const Cylinder & | shape, | ||
const Transform3s & | 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.
void coal::generateBVHModel | ( | BVHModel< BV > & | model, |
const Cylinder & | shape, | ||
const Transform3s & | 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.
void coal::generateBVHModel | ( | BVHModel< BV > & | model, |
const Sphere & | shape, | ||
const Transform3s & | 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.
void coal::generateBVHModel | ( | BVHModel< BV > & | model, |
const Sphere & | shape, | ||
const Transform3s & | 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.
void coal::generateCoordinateSystem | ( | const Eigen::MatrixBase< Derived1 > & | _w, |
const Eigen::MatrixBase< Derived2 > & | _u, | ||
const Eigen::MatrixBase< Derived3 > & | _v | ||
) |
|
inline |
Returns the name associated to a NODE_TYPE.
|
inline |
Returns the name associated to a OBJECT_TYPE.
void coal::getCovariance | ( | Vec3s * | ps, |
Vec3s * | ps2, | ||
Triangle32 * | ts, | ||
unsigned int * | indices, | ||
unsigned int | n, | ||
Matrix3s & | 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.
void coal::getExtentAndCenter | ( | Vec3s * | ps, |
Vec3s * | ps2, | ||
Triangle32 * | ts, | ||
unsigned int * | indices, | ||
unsigned int | n, | ||
Matrix3s & | axes, | ||
Vec3s & | center, | ||
Vec3s & | extent | ||
) |
Compute the bounding volume extent and center for a set or subset of points, given the BV axises.
void coal::getRadiusAndOriginAndRectangleSize | ( | Vec3s * | ps, |
Vec3s * | ps2, | ||
Triangle32 * | ts, | ||
unsigned int * | indices, | ||
unsigned int | n, | ||
const Matrix3s & | axes, | ||
Vec3s & | origin, | ||
Scalar | l[2], | ||
Scalar & | r | ||
) |
Compute the RSS bounding volume parameters: radius, rectangle size and the origin, given the BV axises.
bool coal::isEqual | ( | const Eigen::MatrixBase< Derived > & | lhs, |
const Eigen::MatrixBase< OtherDerived > & | rhs, | ||
const Scalar | tol = std::numeric_limits<Scalar>::epsilon() * 100 |
||
) |
|
inline |
Read a mesh file and convert it to a polyhedral mesh.
[in] | resource_path | Path to the ressource mesh file to be read |
[in] | scale | Scale to apply when reading the ressource |
[out] | polyhedron | The resulted polyhedron |
OcTreePtr_t coal::makeOctree | ( | const Eigen::Matrix< Scalar, Eigen::Dynamic, 3 > & | point_cloud, |
const Scalar | resolution | ||
) |
Scalar coal::maximumDistance | ( | Vec3s * | ps, |
Vec3s * | ps2, | ||
Triangle32 * | ts, | ||
unsigned int * | indices, | ||
unsigned int | n, | ||
const Vec3s & | query | ||
) |
Compute the maximum distance from a given center point to a point cloud.
Check collision between two boxes
B,T | orientation and position of first box, |
a | half dimensions of first box, |
b | half dimensions of second box. The second box is in identity configuration. |
bool coal::obbDisjointAndLowerBoundDistance | ( | const Matrix3s & | B, |
const Vec3s & | T, | ||
const Vec3s & | a, | ||
const Vec3s & | b, | ||
const CollisionRequest & | request, | ||
Scalar & | squaredLowerBoundDistance | ||
) |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
bool coal::overlap | ( | const Matrix3s & | , |
const Vec3s & | , | ||
const KDOP< N > & | , | ||
const KDOP< N > & | |||
) |
bool coal::overlap | ( | const Matrix3s & | , |
const Vec3s & | , | ||
const KDOP< N > & | , | ||
const KDOP< N > & | , | ||
const CollisionRequest & | , | ||
Scalar & | |||
) |
Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool coal::overlap | ( | const Matrix3s & | R0, |
const Vec3s & | T0, | ||
const AABB & | b1, | ||
const AABB & | b2, | ||
const CollisionRequest & | request, | ||
Scalar & | sqrDistLowerBound | ||
) |
Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity.
bool coal::overlap | ( | const Matrix3s & | R0, |
const Vec3s & | T0, | ||
const kIOS & | b1, | ||
const kIOS & | b2, | ||
const CollisionRequest & | request, | ||
Scalar & | sqrDistLowerBound | ||
) |
Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity.
Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool coal::overlap | ( | const Matrix3s & | R0, |
const Vec3s & | T0, | ||
const OBB & | b1, | ||
const OBB & | b2, | ||
const CollisionRequest & | request, | ||
Scalar & | sqrDistLowerBound | ||
) |
Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity.
|
inline |
Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity.
Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
bool coal::overlap | ( | const Matrix3s & | R0, |
const Vec3s & | T0, | ||
const RSS & | b1, | ||
const RSS & | b2, | ||
const CollisionRequest & | request, | ||
Scalar & | sqrDistLowerBound | ||
) |
Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
void coal::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 coal::reorderTriangle | ( | const ConvexTpl< TriangleTpl< IndexType > > * | convex_tri, |
TriangleTpl< IndexType > & | tri | ||
) |
void coal::ShapeShapeContactPatch | ( | const CollisionGeometry * | o1, |
const Transform3s & | tf1, | ||
const CollisionGeometry * | o2, | ||
const Transform3s & | tf2, | ||
const CollisionResult & | collision_result, | ||
const ContactPatchSolver * | csolver, | ||
const ContactPatchRequest & | request, | ||
ContactPatchResult & | result | ||
) |
Halfspace coal::transform | ( | const Halfspace & | a, |
const Transform3s & | tf | ||
) |
Plane coal::transform | ( | const Plane & | a, |
const Transform3s & | tf | ||
) |
std::array< Halfspace, 2 > coal::transformToHalfspaces | ( | const Plane & | a, |
const Transform3s & | tf | ||
) |
translate the KDOP BV
|
inline |
Uniformly random quaternion sphere. Code taken from Pinocchio (https://github.com/stack-of-tasks/pinocchio).
|
inline |
Add new front node into the front list.
|
constexpr |
EPA EPA build a polytope which maximum size is:
#iterations + 4
vertices2 x #iterations + 4
faces
|
constexpr |
GJK.
Note: if the considered shapes are on the order of the meter, and the convergence criterion of GJK is the default VDB criterion, setting a tolerance of 1e-6 on the GJK algorithm makes it precise up to the micro-meter. The same is true for EPA.