Namespaces | |
dynamic_AABB_tree | |
dynamic_AABB_tree_array | |
implementation_array | |
kIOS_fit_functions | |
libccd_extension | |
OBB_fit_functions | |
OBBRSS_fit_functions | |
RSS_fit_functions | |
Enumerations | |
enum | SplitMethodType { SPLIT_METHOD_MEAN, SPLIT_METHOD_MEDIAN, SPLIT_METHOD_BV_CENTER } |
Three types of split algorithms are provided in FCL as default. More... | |
Functions | |
bool | are_coincident (const Vector3d &p, const Vector3d &q) |
::testing::AssertionResult | are_same (const Vector3d &expected, const ccd_vec3_t &tested, double tolerance) |
template int | boxBox2 (const Vector3< double > &side1, const Transform3< double > &tf1, const Vector3< double > &side2, const Transform3< double > &tf2, Vector3< double > &normal, double *depth, int *return_code, int maxc, std::vector< ContactPoint< double >> &contacts) |
template<typename S , typename DerivedA , typename DerivedB > | |
FCL_EXPORT int | boxBox2 (const Vector3< S > &side1, const Eigen::MatrixBase< DerivedA > &R1, const Eigen::MatrixBase< DerivedB > &T1, const Vector3< S > &side2, const Eigen::MatrixBase< DerivedA > &R2, const Eigen::MatrixBase< DerivedB > &T2, Vector3< S > &normal, S *depth, int *return_code, int maxc, std::vector< ContactPoint< S >> &contacts) |
template<typename S , typename DerivedA , typename DerivedB > | |
int | boxBox2 (const Vector3< S > &side1, const Eigen::MatrixBase< DerivedA > &R1, const Eigen::MatrixBase< DerivedB > &T1, const Vector3< S > &side2, const Eigen::MatrixBase< DerivedA > &R2, const Eigen::MatrixBase< DerivedB > &T2, Vector3< S > &normal, S *depth, int *return_code, int maxc, std::vector< ContactPoint< S >> &contacts) |
template<typename S > | |
FCL_EXPORT int | boxBox2 (const Vector3< S > &side1, const Transform3< S > &tf1, const Vector3< S > &side2, const Transform3< S > &tf2, Vector3< S > &normal, S *depth, int *return_code, int maxc, std::vector< ContactPoint< S >> &contacts) |
template<typename S > | |
int | boxBox2 (const Vector3< S > &side1, const Transform3< S > &tf1, const Vector3< S > &side2, const Transform3< S > &tf2, Vector3< S > &normal, S *depth, int *return_code, int maxc, std::vector< ContactPoint< S >> &contacts) |
template bool | boxBoxIntersect (const Box< double > &s1, const Transform3< double > &tf1, const Box< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts_) |
template<typename S > | |
FCL_EXPORT bool | boxBoxIntersect (const Box< S > &s1, const Transform3< S > &tf1, const Box< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts_) |
template<typename S > | |
bool | boxBoxIntersect (const Box< S > &s1, const Transform3< S > &tf1, const Box< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts_) |
template bool | boxHalfspaceIntersect (const Box< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2) |
template bool | boxHalfspaceIntersect (const Box< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts) |
template<typename S > | |
FCL_EXPORT bool | boxHalfspaceIntersect (const Box< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2) |
box half space, a, b, c = +/- edge size n^T * (R(o + a v1 + b v2 + c v3) + T) <= d so (R^T n) (a v1 + b v2 + c v3) + n * T <= d check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c the max value of left side is d - n * T + |(R^T n) (a v1 + b v2 + c v3)|, check that is enough More... | |
template<typename S > | |
bool | boxHalfspaceIntersect (const Box< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2) |
box half space, a, b, c = +/- edge size n^T * (R(o + a v1 + b v2 + c v3) + T) <= d so (R^T n) (a v1 + b v2 + c v3) + n * T <= d check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c the max value of left side is d - n * T + |(R^T n) (a v1 + b v2 + c v3)|, check that is enough More... | |
template<typename S > | |
FCL_EXPORT bool | boxHalfspaceIntersect (const Box< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
bool | boxHalfspaceIntersect (const Box< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template bool | boxPlaneIntersect (const Box< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts) |
template<typename S > | |
FCL_EXPORT bool | boxPlaneIntersect (const Box< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
box half space, a, b, c = +/- edge size n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d so (R^T n) (a v1 + b v2 + c v3) + n * T ~ d check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c and <=0 for some a, b, c so need to check whether |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)|, the reason is as follows: (R^T n) (a v1 + b v2 + c v3) can get |(R^T n) (a v1 + b v2 + c v3)| for one a, b, c. if |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)| then can get both positive and negative value on the right side. More... | |
template<typename S > | |
bool | boxPlaneIntersect (const Box< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
box half space, a, b, c = +/- edge size n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d so (R^T n) (a v1 + b v2 + c v3) + n * T ~ d check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c and <=0 for some a, b, c so need to check whether |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)|, the reason is as follows: (R^T n) (a v1 + b v2 + c v3) can get |(R^T n) (a v1 + b v2 + c v3)| for one a, b, c. if |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)| then can get both positive and negative value on the right side. More... | |
template<typename S > | |
static void | boxToGJK (const Box< S > &s, const Transform3< S > &tf, ccd_box_t *box) |
template<typename BV > | |
std::size_t | BVHCollide (const CollisionGeometry< typename BV::S > *o1, const Transform3< typename BV::S > &tf1, const CollisionGeometry< typename BV::S > *o2, const Transform3< typename BV::S > &tf2, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result) |
template<typename BV , typename NarrowPhaseSolver > | |
std::size_t | BVHCollide (const CollisionGeometry< typename BV::S > *o1, const Transform3< typename BV::S > &tf1, const CollisionGeometry< typename BV::S > *o2, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result) |
template<typename BV , typename NarrowPhaseSolver > | |
BV::S | BVHConservativeAdvancement (const CollisionGeometry< typename BV::S > *o1, const MotionBase< typename BV::S > *motion1, const CollisionGeometry< typename BV::S > *o2, const MotionBase< typename BV::S > *motion2, const NarrowPhaseSolver *nsolver, const ContinuousCollisionRequest< typename BV::S > &request, ContinuousCollisionResult< typename BV::S > &result) |
template<typename BV > | |
BV::S | BVHDistance (const CollisionGeometry< typename BV::S > *o1, const Transform3< typename BV::S > &tf1, const CollisionGeometry< typename BV::S > *o2, const Transform3< typename BV::S > &tf2, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename BV , typename NarrowPhaseSolver > | |
BV::S | BVHDistance (const CollisionGeometry< typename BV::S > *o1, const Transform3< typename BV::S > &tf1, const CollisionGeometry< typename BV::S > *o2, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename BV , typename Shape , typename NarrowPhaseSolver > | |
BV::S | BVHShapeConservativeAdvancement (const CollisionGeometry< typename BV::S > *o1, const MotionBase< typename BV::S > *motion1, const CollisionGeometry< typename BV::S > *o2, const MotionBase< typename BV::S > *motion2, const NarrowPhaseSolver *nsolver, const ContinuousCollisionRequest< typename BV::S > &request, ContinuousCollisionResult< typename BV::S > &result) |
template bool | capsuleCapsuleDistance (const Capsule< double > &s1, const Transform3< double > &tf1, const Capsule< double > &s2, const Transform3< double > &tf2, double *dist, Vector3d *p1_res, Vector3d *p2_res) |
template<typename S > | |
FCL_EXPORT bool | capsuleCapsuleDistance (const Capsule< S > &s1, const Transform3< S > &X_FC1, const Capsule< S > &s2, const Transform3< S > &X_FC2, S *dist, Vector3< S > *p_FW1, Vector3< S > *p_FW2) |
template<typename S > | |
bool | capsuleCapsuleDistance (const Capsule< S > &s1, const Transform3< S > &X_FC1, const Capsule< S > &s2, const Transform3< S > &X_FC2, S *dist, Vector3< S > *p_FW1, Vector3< S > *p_FW2) |
template bool | capsuleHalfspaceIntersect (const Capsule< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts) |
template<typename S > | |
FCL_EXPORT bool | capsuleHalfspaceIntersect (const Capsule< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
bool | capsuleHalfspaceIntersect (const Capsule< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template bool | capsulePlaneIntersect (const Capsule< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2) |
template bool | capsulePlaneIntersect (const Capsule< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts) |
template<typename S > | |
FCL_EXPORT bool | capsulePlaneIntersect (const Capsule< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2) |
template<typename S > | |
bool | capsulePlaneIntersect (const Capsule< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2) |
template<typename S > | |
FCL_EXPORT bool | capsulePlaneIntersect (const Capsule< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
bool | capsulePlaneIntersect (const Capsule< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
static void | capToGJK (const Capsule< S > &s, const Transform3< S > &tf, ccd_cap_t *cap) |
Vector3d | ccd_to_eigen (const ccd_vec3_t &vector) |
template<typename S > | |
static void | centerConvex (const void *obj, ccd_vec3_t *c) |
static void | centerShape (const void *obj, ccd_vec3_t *c) |
static void | centerTriangle (const void *obj, ccd_vec3_t *c) |
void | CheckComputeVisiblePatch (const Polytope &polytope, ccd_pt_face_t &face, const ccd_vec3_t &new_vertex, const std::unordered_set< int > &border_edge_indices_expected, const std::unordered_set< int > &visible_face_indices_expected, const std::unordered_set< int > &internal_edges_indices_expected) |
void | CheckComputeVisiblePatchColinearNewVertex (EquilateralTetrahedron &tet, double rho) |
void | CheckComputeVisiblePatchCommon (const Polytope &polytope, const std::unordered_set< ccd_pt_edge_t * > &border_edges, const std::unordered_set< ccd_pt_face_t * > &visible_faces, const std::unordered_set< ccd_pt_edge_t * > &internal_edges, const std::unordered_set< int > &border_edge_indices_expected, const std::unordered_set< int > &visible_face_indices_expected, const std::unordered_set< int > internal_edges_indices_expected) |
void | CheckComputeVisiblePatchRecursive (const Polytope &polytope, ccd_pt_face_t &face, const std::vector< int > &edge_indices, const ccd_vec3_t &new_vertex, const std::unordered_set< int > &border_edge_indices_expected, const std::unordered_set< int > &visible_face_indices_expected, const std::unordered_set< int > &internal_edges_indices_expected) |
void | CheckTetrahedronFaceNormal (const Tetrahedron &p) |
template double | clamp (double n, double min, double max) |
template<typename S > | |
FCL_EXPORT S | clamp (S n, S min, S max) |
template<typename S > | |
S | clamp (S n, S min, S max) |
template<typename S > | |
FCL_EXPORT S | closestPtSegmentSegment (const Vector3< S > &p_FP1, const Vector3< S > &p_FQ1, const Vector3< S > &p_FP2, const Vector3< S > &p_FQ2, S *s, S *t, Vector3< S > *p_FC1, Vector3< S > *p_FC2) |
template<typename S > | |
S | closestPtSegmentSegment (const Vector3< S > &p_FP1, const Vector3< S > &p_FQ1, const Vector3< S > &p_FP2, const Vector3< S > &p_FQ2, S *s, S *t, Vector3< S > *p_FC1, Vector3< S > *p_FC2) |
template double | closestPtSegmentSegment (const Vector3d &p_FP1, const Vector3d &p_FQ1, const Vector3d &p_FP2, const Vector3d &p_FQ2, double *s, double *t, Vector3d *p_FC1, Vector3d *p_FC2) |
template void | collide (CollisionTraversalNodeBase< double > *node, BVHFrontList *front_list) |
template<typename S > | |
void | collide (CollisionTraversalNodeBase< S > *node, BVHFrontList *front_list) |
collision on collision traversal node; can use front list to accelerate More... | |
template<typename S > | |
FCL_EXPORT void | collide (CollisionTraversalNodeBase< S > *node, BVHFrontList *front_list=nullptr) |
collision on collision traversal node; can use front list to accelerate More... | |
template void | collide2 (MeshCollisionTraversalNodeOBB< double > *node, BVHFrontList *front_list) |
template<typename S > | |
void | collide2 (MeshCollisionTraversalNodeOBB< S > *node, BVHFrontList *front_list) |
special collision on OBB traversal node More... | |
template<typename S > | |
FCL_EXPORT void | collide2 (MeshCollisionTraversalNodeOBB< S > *node, BVHFrontList *front_list=nullptr) |
special collision on OBB traversal node More... | |
template void | collide2 (MeshCollisionTraversalNodeRSS< double > *node, BVHFrontList *front_list) |
template<typename S > | |
void | collide2 (MeshCollisionTraversalNodeRSS< S > *node, BVHFrontList *front_list) |
special collision on RSS traversal node More... | |
template<typename S > | |
FCL_EXPORT void | collide2 (MeshCollisionTraversalNodeRSS< S > *node, BVHFrontList *front_list=nullptr) |
special collision on RSS traversal node More... | |
template void | collisionRecurse (CollisionTraversalNodeBase< double > *node, int b1, int b2, BVHFrontList *front_list) |
template<typename S > | |
FCL_EXPORT void | collisionRecurse (CollisionTraversalNodeBase< S > *node, int b1, int b2, BVHFrontList *front_list) |
Recurse function for collision. More... | |
template void | collisionRecurse (MeshCollisionTraversalNodeOBB< double > *node, int b1, int b2, const Matrix3< double > &R, const Vector3< double > &T, BVHFrontList *front_list) |
template<typename S > | |
FCL_EXPORT void | collisionRecurse (MeshCollisionTraversalNodeOBB< S > *node, int b1, int b2, const Matrix3< S > &R, const Vector3< S > &T, BVHFrontList *front_list) |
Recurse function for collision, specialized for OBB type. More... | |
template void | collisionRecurse (MeshCollisionTraversalNodeRSS< double > *node, int b1, int b2, const Matrix3< double > &R, const Vector3< double > &T, BVHFrontList *front_list) |
template<typename S > | |
FCL_EXPORT void | collisionRecurse (MeshCollisionTraversalNodeRSS< S > *node, int b1, int b2, const Matrix3< S > &R, const Vector3< S > &T, BVHFrontList *front_list) |
Recurse function for collision, specialized for RSS type. More... | |
void | CompareCcdVec3 (const ccd_vec3_t &v, const ccd_vec3_t &v_expected, ccd_real_t tol) |
void | ComparePolytope (const ccd_pt_t *polytope1, const ccd_pt_t *polytope2, ccd_real_t tol) |
template<typename S > | |
S | ComputeSphereSphereDistance (S radius1, S radius2, const Vector3< S > &p_F1, const Vector3< S > &p_F2) |
template<typename S , typename BV > | |
void | computeSplitValue_bvcenter (const BV &bv, S &split_value) |
template<typename S , typename BV > | |
void | computeSplitValue_mean (const BV &bv, Vector3< S > *vertices, Triangle *triangles, unsigned int *primitive_indices, int num_primitives, BVHModelType type, const Vector3< S > &split_vector, S &split_value) |
template<typename S , typename BV > | |
void | computeSplitValue_median (const BV &bv, Vector3< S > *vertices, Triangle *triangles, unsigned int *primitive_indices, int num_primitives, BVHModelType type, const Vector3< S > &split_vector, S &split_value) |
template<typename S , typename BV > | |
void | computeSplitVector (const BV &bv, Vector3< S > &split_vector) |
template bool | coneHalfspaceIntersect (const Cone< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts) |
template<typename S > | |
FCL_EXPORT bool | coneHalfspaceIntersect (const Cone< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
bool | coneHalfspaceIntersect (const Cone< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template bool | conePlaneIntersect (const Cone< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts) |
template<typename S > | |
FCL_EXPORT bool | conePlaneIntersect (const Cone< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
bool | conePlaneIntersect (const Cone< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
static void | coneToGJK (const Cone< S > &s, const Transform3< S > &tf, ccd_cone_t *cone) |
template<typename BV > | |
bool | conservativeAdvancement (const BVHModel< BV > &o1, const MotionBase< typename BV::S > *motion1, const BVHModel< BV > &o2, const MotionBase< typename BV::S > *motion2, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, typename BV::S &toc) |
template<typename BV , typename Shape , typename NarrowPhaseSolver > | |
bool | conservativeAdvancement (const BVHModel< BV > &o1, const MotionBase< typename BV::S > *motion1, const Shape &o2, const MotionBase< typename BV::S > *motion2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, typename BV::S &toc) |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | conservativeAdvancement (const BVHModel< OBBRSS< typename Shape::S >> &o1, const MotionBase< typename Shape::S > *motion1, const Shape &o2, const MotionBase< typename Shape::S > *motion2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result, typename Shape::S &toc) |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | conservativeAdvancement (const BVHModel< RSS< typename Shape::S >> &o1, const MotionBase< typename Shape::S > *motion1, const Shape &o2, const MotionBase< typename Shape::S > *motion2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result, typename Shape::S &toc) |
template<typename Shape , typename BV , typename NarrowPhaseSolver > | |
bool | conservativeAdvancement (const Shape &o1, const MotionBase< typename BV::S > *motion1, const BVHModel< BV > &o2, const MotionBase< typename BV::S > *motion2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, typename BV::S &toc) |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | conservativeAdvancement (const Shape &o1, const MotionBase< typename Shape::S > *motion1, const BVHModel< OBBRSS< typename Shape::S >> &o2, const MotionBase< typename Shape::S > *motion2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result, typename Shape::S &toc) |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | conservativeAdvancement (const Shape &o1, const MotionBase< typename Shape::S > *motion1, const BVHModel< RSS< typename Shape::S >> &o2, const MotionBase< typename Shape::S > *motion2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result, typename Shape::S &toc) |
template<typename Shape1 , typename Shape2 , typename NarrowPhaseSolver > | |
bool | conservativeAdvancement (const Shape1 &o1, const MotionBase< typename Shape1::S > *motion1, const Shape2 &o2, const MotionBase< typename Shape1::S > *motion2, const NarrowPhaseSolver *solver, const CollisionRequest< typename Shape1::S > &request, CollisionResult< typename Shape1::S > &result, typename Shape1::S &toc) |
template<typename BV , typename ConservativeAdvancementOrientedNode > | |
bool | conservativeAdvancementMeshOriented (const BVHModel< BV > &o1, const MotionBase< typename BV::S > *motion1, const BVHModel< BV > &o2, const MotionBase< typename BV::S > *motion2, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, typename BV::S &toc) |
template<typename BV , typename Shape , typename NarrowPhaseSolver , typename ConservativeAdvancementOrientedNode > | |
bool | conservativeAdvancementMeshShapeOriented (const BVHModel< BV > &o1, const MotionBase< typename BV::S > *motion1, const Shape &o2, const MotionBase< typename BV::S > *motion2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, typename BV::S &toc) |
template<typename Shape , typename BV , typename NarrowPhaseSolver , typename ConservativeAdvancementOrientedNode > | |
bool | conservativeAdvancementShapeMeshOriented (const Shape &o1, const MotionBase< typename BV::S > *motion1, const BVHModel< BV > &o2, const MotionBase< typename BV::S > *motion2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, typename BV::S &toc) |
template<typename BV > | |
FCL_EXPORT BV::S | continuousCollideBVHPolynomial (const CollisionGeometry< typename BV::S > *o1_, const TranslationMotion< typename BV::S > *motion1, const CollisionGeometry< typename BV::S > *o2_, const TranslationMotion< typename BV::S > *motion2, const ContinuousCollisionRequest< typename BV::S > &request, ContinuousCollisionResult< typename BV::S > &result) |
template<typename NarrowPhaseSolver > | |
FCL_EXPORT NarrowPhaseSolver::S | continuousCollideConservativeAdvancement (const CollisionGeometry< typename NarrowPhaseSolver::S > *o1, const MotionBase< typename NarrowPhaseSolver::S > *motion1, const CollisionGeometry< typename NarrowPhaseSolver::S > *o2, const MotionBase< typename NarrowPhaseSolver::S > *motion2, const NarrowPhaseSolver *nsolver_, const ContinuousCollisionRequest< typename NarrowPhaseSolver::S > &request, ContinuousCollisionResult< typename NarrowPhaseSolver::S > &result) |
template bool | convexHalfspaceIntersect (const Convex< double > &convex_C, const Transform3< double > &X_FC, const Halfspace< double > &half_space_H, const Transform3< double > &X_FH, std::vector< ContactPoint< double >> *contacts) |
template bool | convexHalfspaceIntersect (const Convex< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, Vector3< double > *contact_points, double *penetration_depth, Vector3< double > *normal) |
template<typename S > | |
FCL_EXPORT bool | convexHalfspaceIntersect (const Convex< S > &convex_C, const Transform3< S > &X_FC, const Halfspace< S > &half_space_H, const Transform3< S > &X_FH, std::vector< ContactPoint< S >> *contacts) |
Reports whether a convex mesh and half space are intersecting. If contacts is not nullptr and the two geometries are intersecting, a new ContactPoint will be added to the data. More... | |
template<typename S > | |
bool | convexHalfspaceIntersect (const Convex< S > &convex_C, const Transform3< S > &X_FC, const Halfspace< S > &half_space_H, const Transform3< S > &X_FH, std::vector< ContactPoint< S >> *contacts) |
Reports whether a convex mesh and half space are intersecting. If contacts is not nullptr and the two geometries are intersecting, a new ContactPoint will be added to the data. More... | |
template<typename S > | |
FCL_EXPORT bool | convexHalfspaceIntersect (const Convex< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal) |
template<typename S > | |
bool | convexHalfspaceIntersect (const Convex< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal) |
template bool | convexPlaneIntersect (const Convex< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, Vector3< double > *contact_points, double *penetration_depth, Vector3< double > *normal) |
template<typename S > | |
FCL_EXPORT bool | convexPlaneIntersect (const Convex< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal) |
template<typename S > | |
bool | convexPlaneIntersect (const Convex< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal) |
template<typename S > | |
static void | convexToGJK (const Convex< S > &s, const Transform3< S > &tf, ccd_convex_t< S > *conv) |
template void | cullPoints2 (int n, double p[], int m, int i0, int iret[]) |
template<typename S > | |
FCL_EXPORT void | cullPoints2 (int n, S p[], int m, int i0, int iret[]) |
template<typename S > | |
void | cullPoints2 (int n, S p[], int m, int i0, int iret[]) |
template bool | cylinderHalfspaceIntersect (const Cylinder< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts) |
template<typename S > | |
FCL_EXPORT bool | cylinderHalfspaceIntersect (const Cylinder< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
bool | cylinderHalfspaceIntersect (const Cylinder< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template bool | cylinderPlaneIntersect (const Cylinder< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2) |
template bool | cylinderPlaneIntersect (const Cylinder< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts) |
template<typename S > | |
FCL_EXPORT bool | cylinderPlaneIntersect (const Cylinder< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2) |
cylinder-plane intersect n^T (R (r * cosa * v1 + r * sina * v2 + h * v3) + T) ~ d need one point to be positive and one to be negative (n^T * v3) * h + n * T -d + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) ~ 0 (n^T * v3) * h + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) + n * T - d ~ 0 More... | |
template<typename S > | |
bool | cylinderPlaneIntersect (const Cylinder< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2) |
cylinder-plane intersect n^T (R (r * cosa * v1 + r * sina * v2 + h * v3) + T) ~ d need one point to be positive and one to be negative (n^T * v3) * h + n * T -d + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) ~ 0 (n^T * v3) * h + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) + n * T - d ~ 0 More... | |
template<typename S > | |
FCL_EXPORT bool | cylinderPlaneIntersect (const Cylinder< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
bool | cylinderPlaneIntersect (const Cylinder< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
static void | cylToGJK (const Cylinder< S > &s, const Transform3< S > &tf, ccd_cyl_t *cyl) |
template void | distance (DistanceTraversalNodeBase< double > *node, BVHFrontList *front_list, int qsize) |
template<typename S > | |
void | distance (DistanceTraversalNodeBase< S > *node, BVHFrontList *front_list, int qsize) |
distance computation on distance traversal node; can use front list to accelerate More... | |
template<typename S > | |
FCL_EXPORT void | distance (DistanceTraversalNodeBase< S > *node, BVHFrontList *front_list=nullptr, int qsize=2) |
distance computation on distance traversal node; can use front list to accelerate More... | |
template<typename BV > | |
FCL_EXPORT void | distancePostprocessOrientedNode (const BVHModel< BV > *model1, const BVHModel< BV > *model2, const Transform3< typename BV::S > &tf1, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename BV > | |
void | distancePostprocessOrientedNode (const BVHModel< BV > *model1, const BVHModel< BV > *model2, const Transform3< typename BV::S > &tf1, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename BV > | |
FCL_EXPORT void | distancePreprocessOrientedNode (const BVHModel< BV > *model1, const BVHModel< BV > *model2, const Vector3< typename BV::S > *vertices1, Vector3< typename BV::S > *vertices2, Triangle *tri_indices1, Triangle *tri_indices2, int init_tri_id1, int init_tri_id2, const Matrix3< typename BV::S > &R, const Vector3< typename BV::S > &T, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename BV > | |
void | distancePreprocessOrientedNode (const BVHModel< BV > *model1, const BVHModel< BV > *model2, const Vector3< typename BV::S > *vertices1, Vector3< typename BV::S > *vertices2, Triangle *tri_indices1, Triangle *tri_indices2, int init_tri_id1, int init_tri_id2, const Matrix3< typename BV::S > &R, const Vector3< typename BV::S > &T, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename BV > | |
FCL_EXPORT void | distancePreprocessOrientedNode (const BVHModel< BV > *model1, const BVHModel< BV > *model2, const Vector3< typename BV::S > *vertices1, Vector3< typename BV::S > *vertices2, Triangle *tri_indices1, Triangle *tri_indices2, int init_tri_id1, int init_tri_id2, const Transform3< typename BV::S > &tf, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename BV > | |
void | distancePreprocessOrientedNode (const BVHModel< BV > *model1, const BVHModel< BV > *model2, const Vector3< typename BV::S > *vertices1, Vector3< typename BV::S > *vertices2, Triangle *tri_indices1, Triangle *tri_indices2, int init_tri_id1, int init_tri_id2, const Transform3< typename BV::S > &tf, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename BV , typename Shape , typename NarrowPhaseSolver > | |
void | distancePreprocessOrientedNode (const BVHModel< BV > *model1, Vector3< typename BV::S > *vertices, Triangle *tri_indices, int init_tri_id, const Shape &model2, const Transform3< typename BV::S > &tf1, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename BV::S > &, DistanceResult< typename BV::S > &result) |
template void | distanceQueueRecurse (DistanceTraversalNodeBase< double > *node, int b1, int b2, BVHFrontList *front_list, int qsize) |
template<typename S > | |
FCL_EXPORT void | distanceQueueRecurse (DistanceTraversalNodeBase< S > *node, int b1, int b2, BVHFrontList *front_list, int qsize) |
Recurse function for distance, using queue acceleration. More... | |
template void | distanceRecurse (DistanceTraversalNodeBase< double > *node, int b1, int b2, BVHFrontList *front_list) |
template<typename S > | |
FCL_EXPORT void | distanceRecurse (DistanceTraversalNodeBase< S > *node, int b1, int b2, BVHFrontList *front_list) |
Recurse function for distance. More... | |
bool | EdgeMatch (const ccd_pt_edge_t *e1, const ccd_pt_edge_t *e2, const std::unordered_map< ccd_pt_vertex_t *, ccd_pt_vertex_t * > &map_v1_to_v2) |
ccd_vec3_t | eigen_to_ccd (const Vector3d &vector) |
template bool | ellipsoidHalfspaceIntersect (const Ellipsoid< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts) |
template<typename S > | |
FCL_EXPORT bool | ellipsoidHalfspaceIntersect (const Ellipsoid< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
bool | ellipsoidHalfspaceIntersect (const Ellipsoid< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template bool | ellipsoidPlaneIntersect (const Ellipsoid< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts) |
template<typename S > | |
FCL_EXPORT bool | ellipsoidPlaneIntersect (const Ellipsoid< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
bool | ellipsoidPlaneIntersect (const Ellipsoid< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
static void | ellipsoidToGJK (const Ellipsoid< S > &s, const Transform3< S > &tf, ccd_ellipsoid_t *ellipsoid) |
std::array< fcl::Vector3< ccd_real_t >, 4 > | EquilateralTetrahedronVertices (ccd_real_t bottom_center_x, ccd_real_t bottom_center_y, ccd_real_t bottom_center_z, ccd_real_t edge_length) |
template<typename S , typename BV > | |
const FCL_EXPORT Vector3< S > | getBVAxis (const BV &bv, int i) |
template<typename BV > | |
const Vector3< typename BV::S > | getBVAxis (const BV &bv, int i) |
template<typename S > | |
FCL_EXPORT void | getExtentAndCenter_mesh (const Vector3< S > *const ps, const Vector3< S > *const ps2, Triangle *ts, unsigned int *indices, int n, const Matrix3< S > &axis, Vector3< S > ¢er, Vector3< S > &extent) |
Compute the bounding volume extent and center for a set or subset of points. The bounding volume axes are known. More... | |
template void | getExtentAndCenter_mesh (const Vector3d *const ps, const Vector3d *const ps2, Triangle *ts, unsigned int *indices, int n, const Matrix3d &axis, Vector3d ¢er, Vector3d &extent) |
template<typename S > | |
FCL_EXPORT void | getExtentAndCenter_pointcloud (const Vector3< S > *const ps, const Vector3< S > *const ps2, unsigned int *indices, int n, const Matrix3< S > &axis, Vector3< S > ¢er, Vector3< S > &extent) |
Compute the bounding volume extent and center for a set or subset of points. The bounding volume axes are known. More... | |
template void | getExtentAndCenter_pointcloud (const Vector3d *const ps, const Vector3d *const ps2, unsigned int *indices, int n, const Matrix3d &axis, Vector3d ¢er, Vector3d &extent) |
template<typename S , typename Derived > | |
Vector3< S > | getSupport (const ShapeBase< S > *shape, const Eigen::MatrixBase< Derived > &dir) |
the support function for shape More... | |
template<typename S , typename Derived > | |
FCL_EXPORT Vector3< S > | getSupport (const ShapeBase< S > *shape, const Eigen::MatrixBase< Derived > &dir) |
the support function for shape More... | |
template bool | GJKCollide (void *obj1, ccd_support_fn supp1, ccd_center_fn cen1, void *obj2, ccd_support_fn supp2, ccd_center_fn cen2, unsigned int max_iterations, double tolerance, Vector3d *contact_points, double *penetration_depth, Vector3d *normal) |
template<typename S > | |
FCL_EXPORT bool | GJKCollide (void *obj1, ccd_support_fn supp1, ccd_center_fn cen1, void *obj2, ccd_support_fn supp2, ccd_center_fn cen2, unsigned int max_iterations, S tolerance, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal) |
GJK collision algorithm. More... | |
template<typename S > | |
bool | GJKCollide (void *obj1, ccd_support_fn supp1, ccd_center_fn cen1, void *obj2, ccd_support_fn supp2, ccd_center_fn cen2, unsigned int max_iterations, S tolerance, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal) |
GJK collision algorithm. More... | |
template bool | GJKDistance (void *obj1, ccd_support_fn supp1, void *obj2, ccd_support_fn supp2, unsigned int max_iterations, double tolerance, double *dist, Vector3d *p1, Vector3d *p2) |
template<typename S > | |
FCL_EXPORT bool | GJKDistance (void *obj1, ccd_support_fn supp1, void *obj2, ccd_support_fn supp2, unsigned int max_iterations, S tolerance, S *dist, Vector3< S > *p1, Vector3< S > *p2) |
template<typename S > | |
bool | GJKDistance (void *obj1, ccd_support_fn supp1, void *obj2, ccd_support_fn supp2, unsigned int max_iterations, S tolerance, S *res, Vector3< S > *p1, Vector3< S > *p2) |
template<typename S > | |
bool | GJKDistanceImpl (void *obj1, ccd_support_fn supp1, void *obj2, ccd_support_fn supp2, unsigned int max_iterations, S tolerance, detail::DistanceFn distance_func, S *res, Vector3< S > *p1, Vector3< S > *p2) |
template bool | GJKSignedDistance (void *obj1, ccd_support_fn supp1, void *obj2, ccd_support_fn supp2, unsigned int max_iterations, double tolerance, double *dist, Vector3d *p1, Vector3d *p2) |
template<typename S > | |
FCL_EXPORT bool | GJKSignedDistance (void *obj1, ccd_support_fn supp1, void *obj2, ccd_support_fn supp2, unsigned int max_iterations, S tolerance, S *dist, Vector3< S > *p1, Vector3< S > *p2) |
template<typename S > | |
bool | GJKSignedDistance (void *obj1, ccd_support_fn supp1, void *obj2, ccd_support_fn supp2, unsigned int max_iterations, S tolerance, S *res, Vector3< S > *p1, Vector3< S > *p2) |
GTEST_TEST (DegenerateGeometry, CoincidentPoints) | |
GTEST_TEST (DegenerateGeometry, ZeroAreaTriangle) | |
GTEST_TEST (FCL_GJK_EPA, ComputeVisiblePatch_2FacesVisible) | |
GTEST_TEST (FCL_GJK_EPA, ComputeVisiblePatch_4FacesVisible) | |
GTEST_TEST (FCL_GJK_EPA, ComputeVisiblePatch_TopAndSideFacesVisible) | |
GTEST_TEST (FCL_GJK_EPA, ComputeVisiblePatch_TopFaceVisible) | |
GTEST_TEST (FCL_GJK_EPA, ComputeVisiblePatchColinearNewVertex) | |
GTEST_TEST (FCL_GJK_EPA, ComputeVisiblePatchSanityCheck) | |
GTEST_TEST (FCL_GJK_EPA, convert2SimplexToTetrahedron) | |
GTEST_TEST (FCL_GJK_EPA, expandPolytope_error) | |
GTEST_TEST (FCL_GJK_EPA, expandPolytope_hexagram_1visible_face) | |
GTEST_TEST (FCL_GJK_EPA, expandPolytope_hexagram_4_visible_faces) | |
GTEST_TEST (FCL_GJK_EPA, expandPolytope_tetrahedron1) | |
GTEST_TEST (FCL_GJK_EPA, expandPolytope_tetrahedron_2visible_faces) | |
GTEST_TEST (FCL_GJK_EPA, faceNormalPointingOutward) | |
GTEST_TEST (FCL_GJK_EPA, faceNormalPointingOutwardError) | |
GTEST_TEST (FCL_GJK_EPA, faceNormalPointingOutwardOriginNearFace1) | |
GTEST_TEST (FCL_GJK_EPA, faceNormalPointingOutwardOriginNearFace2) | |
GTEST_TEST (FCL_GJK_EPA, isOutsidePolytopeFace) | |
GTEST_TEST (FCL_GJK_EPA, isOutsidePolytopeFaceError) | |
GTEST_TEST (FCL_GJK_EPA, penEPAPosClosest_edge) | |
GTEST_TEST (FCL_GJK_EPA, penEPAPosClosest_face) | |
GTEST_TEST (FCL_GJK_EPA, penEPAPosClosest_vertex) | |
GTEST_TEST (FCL_GJK_EPA, supportEPADirection) | |
GTEST_TEST (FCL_GJK_EPA, supportEPADirectionError) | |
GTEST_TEST (FCL_GJKSignedDistance, box_box) | |
GTEST_TEST (FCL_GJKSignedDistance, sphere_sphere) | |
template bool | halfspaceIntersect (const Halfspace< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, Vector3< double > &p, Vector3< double > &d, Halfspace< double > &s, double &penetration_depth, int &ret) |
template<typename S > | |
FCL_EXPORT bool | halfspaceIntersect (const Halfspace< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, Vector3< S > &p, Vector3< S > &d, Halfspace< S > &s, S &penetration_depth, int &ret) |
return whether two halfspace intersect if the separation planes of the two halfspaces are parallel return code 1, if two halfspaces' normal are same and s1 is in s2, also return s1 in s; return code 2, if two halfspaces' normal are same and s2 is in s1, also return s2 in s; return code 3, if two halfspaces' normal are opposite and s1 and s2 are into each other; collision free, if two halfspaces' are separate; if the separation planes of the two halfspaces are not parallel, return intersection ray, return code 4. ray origin is p and direction is d collision free return code 0 More... | |
template<typename S > | |
bool | halfspaceIntersect (const Halfspace< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, Vector3< S > &p, Vector3< S > &d, Halfspace< S > &s, S &penetration_depth, int &ret) |
return whether two halfspace intersect if the separation planes of the two halfspaces are parallel return code 1, if two halfspaces' normal are same and s1 is in s2, also return s1 in s; return code 2, if two halfspaces' normal are same and s2 is in s1, also return s2 in s; return code 3, if two halfspaces' normal are opposite and s1 and s2 are into each other; collision free, if two halfspaces' are separate; if the separation planes of the two halfspaces are not parallel, return intersection ray, return code 4. ray origin is p and direction is d collision free return code 0 More... | |
template<> | |
FCL_EXPORT float | halfspaceIntersectTolerance () |
template<typename S > | |
FCL_EXPORT S | halfspaceIntersectTolerance () |
template<typename S > | |
S | halfspaceIntersectTolerance () |
template bool | halfspacePlaneIntersect (const Halfspace< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, Plane< double > &pl, Vector3< double > &p, Vector3< double > &d, double &penetration_depth, int &ret) |
template<typename S > | |
FCL_EXPORT bool | halfspacePlaneIntersect (const Halfspace< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, Plane< S > &pl, Vector3< S > &p, Vector3< S > &d, S &penetration_depth, int &ret) |
template<typename S > | |
bool | halfspacePlaneIntersect (const Halfspace< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, Plane< S > &pl, Vector3< S > &p, Vector3< S > &d, S &penetration_depth, int &ret) |
template bool | halfspaceTriangleIntersect (const Halfspace< double > &s1, const Transform3< double > &tf1, const Vector3< double > &P1, const Vector3< double > &P2, const Vector3< double > &P3, const Transform3< double > &tf2, Vector3< double > *contact_points, double *penetration_depth, Vector3< double > *normal) |
template<typename S > | |
FCL_EXPORT bool | halfspaceTriangleIntersect (const Halfspace< S > &s1, const Transform3< S > &tf1, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Transform3< S > &tf2, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal) |
template<typename S > | |
bool | halfspaceTriangleIntersect (const Halfspace< S > &s1, const Transform3< S > &tf1, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Transform3< S > &tf2, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal) |
template<typename BV > | |
bool | initialize (MeshCollisionTraversalNode< BV > &node, BVHModel< BV > &model1, Transform3< typename BV::S > &tf1, BVHModel< BV > &model2, Transform3< typename BV::S > &tf2, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, bool use_refit, bool refit_bottomup) |
Initialize traversal node for collision between two meshes, given the current transforms. More... | |
template<typename BV > | |
FCL_EXPORT bool | initialize (MeshCollisionTraversalNode< BV > &node, BVHModel< BV > &model1, Transform3< typename BV::S > &tf1, BVHModel< BV > &model2, Transform3< typename BV::S > &tf2, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, bool use_refit=false, bool refit_bottomup=false) |
Initialize traversal node for collision between two meshes, given the current transforms. More... | |
template bool | initialize (MeshCollisionTraversalNodekIOS< double > &node, const BVHModel< kIOS< double >> &model1, const Transform3< double > &tf1, const BVHModel< kIOS< double >> &model2, const Transform3< double > &tf2, const CollisionRequest< double > &request, CollisionResult< double > &result) |
template<typename S > | |
FCL_EXPORT bool | initialize (MeshCollisionTraversalNodekIOS< S > &node, const BVHModel< kIOS< S >> &model1, const Transform3< S > &tf1, const BVHModel< kIOS< S >> &model2, const Transform3< S > &tf2, const CollisionRequest< S > &request, CollisionResult< S > &result) |
Initialize traversal node for collision between two meshes, specialized for kIOS type. More... | |
template<typename S > | |
bool | initialize (MeshCollisionTraversalNodekIOS< S > &node, const BVHModel< kIOS< S >> &model1, const Transform3< S > &tf1, const BVHModel< kIOS< S >> &model2, const Transform3< S > &tf2, const CollisionRequest< S > &request, CollisionResult< S > &result) |
Initialize traversal node for collision between two meshes, specialized for kIOS type. More... | |
template bool | initialize (MeshCollisionTraversalNodeOBB< double > &node, const BVHModel< OBB< double >> &model1, const Transform3< double > &tf1, const BVHModel< OBB< double >> &model2, const Transform3< double > &tf2, const CollisionRequest< double > &request, CollisionResult< double > &result) |
template<typename S > | |
FCL_EXPORT bool | initialize (MeshCollisionTraversalNodeOBB< S > &node, const BVHModel< OBB< S >> &model1, const Transform3< S > &tf1, const BVHModel< OBB< S >> &model2, const Transform3< S > &tf2, const CollisionRequest< S > &request, CollisionResult< S > &result) |
Initialize traversal node for collision between two meshes, specialized for OBB type. More... | |
template<typename S > | |
bool | initialize (MeshCollisionTraversalNodeOBB< S > &node, const BVHModel< OBB< S >> &model1, const Transform3< S > &tf1, const BVHModel< OBB< S >> &model2, const Transform3< S > &tf2, const CollisionRequest< S > &request, CollisionResult< S > &result) |
Initialize traversal node for collision between two meshes, specialized for OBB type. More... | |
template bool | initialize (MeshCollisionTraversalNodeOBBRSS< double > &node, const BVHModel< OBBRSS< double >> &model1, const Transform3< double > &tf1, const BVHModel< OBBRSS< double >> &model2, const Transform3< double > &tf2, const CollisionRequest< double > &request, CollisionResult< double > &result) |
template<typename S > | |
FCL_EXPORT bool | initialize (MeshCollisionTraversalNodeOBBRSS< S > &node, const BVHModel< OBBRSS< S >> &model1, const Transform3< S > &tf1, const BVHModel< OBBRSS< S >> &model2, const Transform3< S > &tf2, const CollisionRequest< S > &request, CollisionResult< S > &result) |
Initialize traversal node for collision between two meshes, specialized for OBBRSS type. More... | |
template<typename S > | |
bool | initialize (MeshCollisionTraversalNodeOBBRSS< S > &node, const BVHModel< OBBRSS< S >> &model1, const Transform3< S > &tf1, const BVHModel< OBBRSS< S >> &model2, const Transform3< S > &tf2, const CollisionRequest< S > &request, CollisionResult< S > &result) |
Initialize traversal node for collision between two meshes, specialized for OBBRSS type. More... | |
template bool | initialize (MeshCollisionTraversalNodeRSS< double > &node, const BVHModel< RSS< double >> &model1, const Transform3< double > &tf1, const BVHModel< RSS< double >> &model2, const Transform3< double > &tf2, const CollisionRequest< double > &request, CollisionResult< double > &result) |
template<typename S > | |
FCL_EXPORT bool | initialize (MeshCollisionTraversalNodeRSS< S > &node, const BVHModel< RSS< S >> &model1, const Transform3< S > &tf1, const BVHModel< RSS< S >> &model2, const Transform3< S > &tf2, const CollisionRequest< S > &request, CollisionResult< S > &result) |
Initialize traversal node for collision between two meshes, specialized for RSS type. More... | |
template<typename S > | |
bool | initialize (MeshCollisionTraversalNodeRSS< S > &node, const BVHModel< RSS< S >> &model1, const Transform3< S > &tf1, const BVHModel< RSS< S >> &model2, const Transform3< S > &tf2, const CollisionRequest< S > &request, CollisionResult< S > &result) |
Initialize traversal node for collision between two meshes, specialized for RSS type. More... | |
template<typename BV > | |
bool | initialize (MeshConservativeAdvancementTraversalNode< BV > &node, BVHModel< BV > &model1, const Transform3< typename BV::S > &tf1, BVHModel< BV > &model2, const Transform3< typename BV::S > &tf2, typename BV::S w, bool use_refit, bool refit_bottomup) |
Initialize traversal node for conservative advancement computation between two meshes, given the current transforms. More... | |
template<typename BV > | |
FCL_EXPORT bool | initialize (MeshConservativeAdvancementTraversalNode< BV > &node, BVHModel< BV > &model1, const Transform3< typename BV::S > &tf1, BVHModel< BV > &model2, const Transform3< typename BV::S > &tf2, typename BV::S w=1, bool use_refit=false, bool refit_bottomup=false) |
Initialize traversal node for conservative advancement computation between two meshes, given the current transforms. More... | |
template bool | initialize (MeshConservativeAdvancementTraversalNodeOBBRSS< double > &node, const BVHModel< OBBRSS< double >> &model1, const Transform3< double > &tf1, const BVHModel< OBBRSS< double >> &model2, const Transform3< double > &tf2, double w) |
template<typename S > | |
bool | initialize (MeshConservativeAdvancementTraversalNodeOBBRSS< S > &node, const BVHModel< OBBRSS< S >> &model1, const Transform3< S > &tf1, const BVHModel< OBBRSS< S >> &model2, const Transform3< S > &tf2, S w) |
template<typename S > | |
FCL_EXPORT bool | initialize (MeshConservativeAdvancementTraversalNodeOBBRSS< S > &node, const BVHModel< OBBRSS< S >> &model1, const Transform3< S > &tf1, const BVHModel< OBBRSS< S >> &model2, const Transform3< S > &tf2, S w=1) |
template bool | initialize (MeshConservativeAdvancementTraversalNodeRSS< double > &node, const BVHModel< RSS< double >> &model1, const Transform3< double > &tf1, const BVHModel< RSS< double >> &model2, const Transform3< double > &tf2, double w) |
template<typename S > | |
bool | initialize (MeshConservativeAdvancementTraversalNodeRSS< S > &node, const BVHModel< RSS< S >> &model1, const Transform3< S > &tf1, const BVHModel< RSS< S >> &model2, const Transform3< S > &tf2, S w) |
Initialize traversal node for conservative advancement computation between two meshes, given the current transforms, specialized for RSS. More... | |
template<typename S > | |
FCL_EXPORT bool | initialize (MeshConservativeAdvancementTraversalNodeRSS< S > &node, const BVHModel< RSS< S >> &model1, const Transform3< S > &tf1, const BVHModel< RSS< S >> &model2, const Transform3< S > &tf2, S w=1) |
Initialize traversal node for conservative advancement computation between two meshes, given the current transforms, specialized for RSS. More... | |
template<typename BV > | |
FCL_EXPORT bool | initialize (MeshContinuousCollisionTraversalNode< BV > &node, const BVHModel< BV > &model1, const Transform3< typename BV::S > &tf1, const BVHModel< BV > &model2, const Transform3< typename BV::S > &tf2, const CollisionRequest< typename BV::S > &request) |
Initialize traversal node for continuous collision detection between two meshes. More... | |
template<typename BV > | |
bool | initialize (MeshContinuousCollisionTraversalNode< BV > &node, const BVHModel< BV > &model1, const Transform3< typename BV::S > &tf1, const BVHModel< BV > &model2, const Transform3< typename BV::S > &tf2, const CollisionRequest< typename BV::S > &request) |
Initialize traversal node for continuous collision detection between two meshes. More... | |
template<typename BV > | |
bool | initialize (MeshDistanceTraversalNode< BV > &node, BVHModel< BV > &model1, Transform3< typename BV::S > &tf1, BVHModel< BV > &model2, Transform3< typename BV::S > &tf2, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result, bool use_refit, bool refit_bottomup) |
Initialize traversal node for distance computation between two meshes, given the current transforms. More... | |
template<typename BV > | |
FCL_EXPORT bool | initialize (MeshDistanceTraversalNode< BV > &node, BVHModel< BV > &model1, Transform3< typename BV::S > &tf1, BVHModel< BV > &model2, Transform3< typename BV::S > &tf2, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result, bool use_refit=false, bool refit_bottomup=false) |
Initialize traversal node for distance computation between two meshes, given the current transforms. More... | |
template bool | initialize (MeshDistanceTraversalNodekIOS< double > &node, const BVHModel< kIOS< double >> &model1, const Transform3< double > &tf1, const BVHModel< kIOS< double >> &model2, const Transform3< double > &tf2, const DistanceRequest< double > &request, DistanceResult< double > &result) |
template<typename S > | |
FCL_EXPORT bool | initialize (MeshDistanceTraversalNodekIOS< S > &node, const BVHModel< kIOS< S >> &model1, const Transform3< S > &tf1, const BVHModel< kIOS< S >> &model2, const Transform3< S > &tf2, const DistanceRequest< S > &request, DistanceResult< S > &result) |
Initialize traversal node for distance computation between two meshes, specialized for kIOS type. More... | |
template<typename S > | |
bool | initialize (MeshDistanceTraversalNodekIOS< S > &node, const BVHModel< kIOS< S >> &model1, const Transform3< S > &tf1, const BVHModel< kIOS< S >> &model2, const Transform3< S > &tf2, const DistanceRequest< S > &request, DistanceResult< S > &result) |
Initialize traversal node for distance computation between two meshes, specialized for kIOS type. More... | |
template bool | initialize (MeshDistanceTraversalNodeOBBRSS< double > &node, const BVHModel< OBBRSS< double >> &model1, const Transform3< double > &tf1, const BVHModel< OBBRSS< double >> &model2, const Transform3< double > &tf2, const DistanceRequest< double > &request, DistanceResult< double > &result) |
template<typename S > | |
FCL_EXPORT bool | initialize (MeshDistanceTraversalNodeOBBRSS< S > &node, const BVHModel< OBBRSS< S >> &model1, const Transform3< S > &tf1, const BVHModel< OBBRSS< S >> &model2, const Transform3< S > &tf2, const DistanceRequest< S > &request, DistanceResult< S > &result) |
Initialize traversal node for distance computation between two meshes, specialized for OBBRSS type. More... | |
template<typename S > | |
bool | initialize (MeshDistanceTraversalNodeOBBRSS< S > &node, const BVHModel< OBBRSS< S >> &model1, const Transform3< S > &tf1, const BVHModel< OBBRSS< S >> &model2, const Transform3< S > &tf2, const DistanceRequest< S > &request, DistanceResult< S > &result) |
Initialize traversal node for distance computation between two meshes, specialized for OBBRSS type. More... | |
template bool | initialize (MeshDistanceTraversalNodeRSS< double > &node, const BVHModel< RSS< double >> &model1, const Transform3< double > &tf1, const BVHModel< RSS< double >> &model2, const Transform3< double > &tf2, const DistanceRequest< double > &request, DistanceResult< double > &result) |
template<typename S > | |
FCL_EXPORT bool | initialize (MeshDistanceTraversalNodeRSS< S > &node, const BVHModel< RSS< S >> &model1, const Transform3< S > &tf1, const BVHModel< RSS< S >> &model2, const Transform3< S > &tf2, const DistanceRequest< S > &request, DistanceResult< S > &result) |
Initialize traversal node for distance computation between two meshes, specialized for RSS type. More... | |
template<typename S > | |
bool | initialize (MeshDistanceTraversalNodeRSS< S > &node, const BVHModel< RSS< S >> &model1, const Transform3< S > &tf1, const BVHModel< RSS< S >> &model2, const Transform3< S > &tf2, const DistanceRequest< S > &request, DistanceResult< S > &result) |
Initialize traversal node for distance computation between two meshes, specialized for RSS type. More... | |
template<typename BV , typename NarrowPhaseSolver > | |
bool | initialize (MeshOcTreeCollisionTraversalNode< BV, NarrowPhaseSolver > &node, const BVHModel< BV > &model1, const Transform3< typename BV::S > &tf1, const OcTree< typename BV::S > &model2, const Transform3< typename BV::S > &tf2, const OcTreeSolver< NarrowPhaseSolver > *otsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result) |
Initialize traversal node for collision between one mesh and one octree, given current object transform. More... | |
template<typename BV , typename NarrowPhaseSolver > | |
bool | initialize (MeshOcTreeDistanceTraversalNode< BV, NarrowPhaseSolver > &node, const BVHModel< BV > &model1, const Transform3< typename BV::S > &tf1, const OcTree< typename BV::S > &model2, const Transform3< typename BV::S > &tf2, const OcTreeSolver< NarrowPhaseSolver > *otsolver, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
Initialize traversal node for distance between one mesh and one octree, given current object transform. More... | |
template<typename BV , typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (MeshShapeCollisionTraversalNode< BV, Shape, NarrowPhaseSolver > &node, BVHModel< BV > &model1, Transform3< typename BV::S > &tf1, const Shape &model2, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, bool use_refit, bool refit_bottomup) |
Initialize traversal node for collision between one mesh and one shape, given current object transform. More... | |
template<typename BV , typename Shape , typename NarrowPhaseSolver > | |
FCL_EXPORT bool | initialize (MeshShapeCollisionTraversalNode< BV, Shape, NarrowPhaseSolver > &node, BVHModel< BV > &model1, Transform3< typename BV::S > &tf1, const Shape &model2, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, bool use_refit=false, bool refit_bottomup=false) |
Initialize traversal node for collision between one mesh and one shape, given current object transform. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
FCL_EXPORT bool | initialize (MeshShapeCollisionTraversalNodekIOS< Shape, NarrowPhaseSolver > &node, const BVHModel< kIOS< typename Shape::S >> &model1, const Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result) |
Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (MeshShapeCollisionTraversalNodekIOS< Shape, NarrowPhaseSolver > &node, const BVHModel< kIOS< typename Shape::S >> &model1, const Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result) |
Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
FCL_EXPORT bool | initialize (MeshShapeCollisionTraversalNodeOBB< Shape, NarrowPhaseSolver > &node, const BVHModel< OBB< typename Shape::S >> &model1, const Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result) |
Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (MeshShapeCollisionTraversalNodeOBB< Shape, NarrowPhaseSolver > &node, const BVHModel< OBB< typename Shape::S >> &model1, const Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result) |
Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
FCL_EXPORT bool | initialize (MeshShapeCollisionTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > &node, const BVHModel< OBBRSS< typename Shape::S >> &model1, const Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result) |
Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (MeshShapeCollisionTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > &node, const BVHModel< OBBRSS< typename Shape::S >> &model1, const Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result) |
Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
FCL_EXPORT bool | initialize (MeshShapeCollisionTraversalNodeRSS< Shape, NarrowPhaseSolver > &node, const BVHModel< RSS< typename Shape::S >> &model1, const Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result) |
Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (MeshShapeCollisionTraversalNodeRSS< Shape, NarrowPhaseSolver > &node, const BVHModel< RSS< typename Shape::S >> &model1, const Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result) |
Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type. More... | |
template<typename BV , typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (MeshShapeConservativeAdvancementTraversalNode< BV, Shape, NarrowPhaseSolver > &node, BVHModel< BV > &model1, const Transform3< typename BV::S > &tf1, const Shape &model2, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, typename BV::S w, bool use_refit, bool refit_bottomup) |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (MeshShapeConservativeAdvancementTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > &node, const BVHModel< OBBRSS< typename Shape::S >> &model1, const Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, typename Shape::S w) |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (MeshShapeConservativeAdvancementTraversalNodeRSS< Shape, NarrowPhaseSolver > &node, const BVHModel< RSS< typename Shape::S >> &model1, const Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, typename Shape::S w) |
template<typename BV , typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (MeshShapeDistanceTraversalNode< BV, Shape, NarrowPhaseSolver > &node, BVHModel< BV > &model1, Transform3< typename BV::S > &tf1, const Shape &model2, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result, bool use_refit=false, bool refit_bottomup=false) |
Initialize traversal node for distance computation between one mesh and one shape, given the current transforms. More... | |
template<typename BV , typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (MeshShapeDistanceTraversalNode< BV, Shape, NarrowPhaseSolver > &node, BVHModel< BV > &model1, Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result, bool use_refit, bool refit_bottomup) |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (MeshShapeDistanceTraversalNodekIOS< Shape, NarrowPhaseSolver > &node, const BVHModel< kIOS< typename Shape::S >> &model1, const Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result) |
Initialize traversal node for distance computation between one mesh and one shape, specialized for kIOS type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (MeshShapeDistanceTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > &node, const BVHModel< OBBRSS< typename Shape::S >> &model1, const Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result) |
Initialize traversal node for distance computation between one mesh and one shape, specialized for OBBRSS type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (MeshShapeDistanceTraversalNodeRSS< Shape, NarrowPhaseSolver > &node, const BVHModel< RSS< typename Shape::S >> &model1, const Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result) |
template<typename NarrowPhaseSolver > | |
bool | initialize (OcTreeCollisionTraversalNode< NarrowPhaseSolver > &node, const OcTree< typename NarrowPhaseSolver::S > &model1, const Transform3< typename NarrowPhaseSolver::S > &tf1, const OcTree< typename NarrowPhaseSolver::S > &model2, const Transform3< typename NarrowPhaseSolver::S > &tf2, const OcTreeSolver< NarrowPhaseSolver > *otsolver, const CollisionRequest< typename NarrowPhaseSolver::S > &request, CollisionResult< typename NarrowPhaseSolver::S > &result) |
Initialize traversal node for collision between two octrees, given current object transform. More... | |
template<typename NarrowPhaseSolver > | |
bool | initialize (OcTreeDistanceTraversalNode< NarrowPhaseSolver > &node, const OcTree< typename NarrowPhaseSolver::S > &model1, const Transform3< typename NarrowPhaseSolver::S > &tf1, const OcTree< typename NarrowPhaseSolver::S > &model2, const Transform3< typename NarrowPhaseSolver::S > &tf2, const OcTreeSolver< NarrowPhaseSolver > *otsolver, const DistanceRequest< typename NarrowPhaseSolver::S > &request, DistanceResult< typename NarrowPhaseSolver::S > &result) |
Initialize traversal node for distance between two octrees, given current object transform. More... | |
template<typename BV , typename NarrowPhaseSolver > | |
bool | initialize (OcTreeMeshCollisionTraversalNode< BV, NarrowPhaseSolver > &node, const OcTree< typename BV::S > &model1, const Transform3< typename BV::S > &tf1, const BVHModel< BV > &model2, const Transform3< typename BV::S > &tf2, const OcTreeSolver< NarrowPhaseSolver > *otsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result) |
Initialize traversal node for collision between one octree and one mesh, given current object transform. More... | |
template<typename BV , typename NarrowPhaseSolver > | |
bool | initialize (OcTreeMeshDistanceTraversalNode< BV, NarrowPhaseSolver > &node, const OcTree< typename BV::S > &model1, const Transform3< typename BV::S > &tf1, const BVHModel< BV > &model2, const Transform3< typename BV::S > &tf2, const OcTreeSolver< NarrowPhaseSolver > *otsolver, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
Initialize traversal node for collision between one octree and one mesh, given current object transform. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (OcTreeShapeCollisionTraversalNode< Shape, NarrowPhaseSolver > &node, const OcTree< typename Shape::S > &model1, const Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename Shape::S > &tf2, const OcTreeSolver< NarrowPhaseSolver > *otsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result) |
Initialize traversal node for collision between one octree and one shape, given current object transform. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (OcTreeShapeDistanceTraversalNode< Shape, NarrowPhaseSolver > &node, const OcTree< typename Shape::S > &model1, const Transform3< typename Shape::S > &tf1, const Shape &model2, const Transform3< typename Shape::S > &tf2, const OcTreeSolver< NarrowPhaseSolver > *otsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result) |
Initialize traversal node for distance between one octree and one shape, given current object transform. More... | |
template<typename Shape1 , typename Shape2 , typename NarrowPhaseSolver > | |
bool | initialize (ShapeCollisionTraversalNode< Shape1, Shape2, NarrowPhaseSolver > &node, const Shape1 &shape1, const Transform3< typename Shape1::S > &tf1, const Shape2 &shape2, const Transform3< typename Shape1::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape1::S > &request, CollisionResult< typename Shape1::S > &result) |
Initialize traversal node for collision between two geometric shapes, given current object transform. More... | |
template<typename Shape1 , typename Shape2 , typename NarrowPhaseSolver > | |
bool | initialize (ShapeConservativeAdvancementTraversalNode< Shape1, Shape2, NarrowPhaseSolver > &node, const Shape1 &shape1, const Transform3< typename Shape1::S > &tf1, const Shape2 &shape2, const Transform3< typename Shape1::S > &tf2, const NarrowPhaseSolver *nsolver) |
template<typename Shape1 , typename Shape2 , typename NarrowPhaseSolver > | |
bool | initialize (ShapeDistanceTraversalNode< Shape1, Shape2, NarrowPhaseSolver > &node, const Shape1 &shape1, const Transform3< typename Shape1::S > &tf1, const Shape2 &shape2, const Transform3< typename Shape1::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape1::S > &request, DistanceResult< typename Shape1::S > &result) |
Initialize traversal node for distance between two geometric shapes. More... | |
template<typename Shape , typename BV , typename NarrowPhaseSolver > | |
FCL_EXPORT bool | initialize (ShapeMeshCollisionTraversalNode< Shape, BV, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename BV::S > &tf1, BVHModel< BV > &model2, Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, bool use_refit=false, bool refit_bottomup=false) |
Initialize traversal node for collision between one mesh and one shape, given current object transform. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
FCL_EXPORT bool | initialize (ShapeMeshCollisionTraversalNodekIOS< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename Shape::S > &tf1, const BVHModel< kIOS< typename Shape::S >> &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result) |
Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
FCL_EXPORT bool | initialize (ShapeMeshCollisionTraversalNodeOBB< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename Shape::S > &tf1, const BVHModel< OBB< typename Shape::S >> &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result) |
Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
FCL_EXPORT bool | initialize (ShapeMeshCollisionTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename Shape::S > &tf1, const BVHModel< OBBRSS< typename Shape::S >> &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result) |
Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
FCL_EXPORT bool | initialize (ShapeMeshCollisionTraversalNodeRSS< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename Shape::S > &tf1, const BVHModel< RSS< typename Shape::S >> &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result) |
Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type. More... | |
template<typename Shape , typename BV , typename NarrowPhaseSolver > | |
bool | initialize (ShapeMeshConservativeAdvancementTraversalNode< Shape, BV, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename BV::S > &tf1, BVHModel< BV > &model2, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, typename BV::S w, bool use_refit, bool refit_bottomup) |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (ShapeMeshConservativeAdvancementTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename Shape::S > &tf1, const BVHModel< OBBRSS< typename Shape::S >> &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, typename Shape::S w) |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (ShapeMeshConservativeAdvancementTraversalNodeRSS< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename Shape::S > &tf1, const BVHModel< RSS< typename Shape::S >> &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, typename Shape::S w) |
template<typename Shape , typename BV , typename NarrowPhaseSolver > | |
FCL_EXPORT bool | initialize (ShapeMeshDistanceTraversalNode< Shape, BV, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename BV::S > &tf1, BVHModel< BV > &model2, Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result, bool use_refit, bool refit_bottomup) |
Initialize traversal node for distance computation between one shape and one mesh, given the current transforms. More... | |
template<typename Shape , typename BV , typename NarrowPhaseSolver > | |
bool | initialize (ShapeMeshDistanceTraversalNode< Shape, BV, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename BV::S > &tf1, BVHModel< BV > &model2, Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result, bool use_refit=false, bool refit_bottomup=false) |
Initialize traversal node for distance computation between one shape and one mesh, given the current transforms. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (ShapeMeshDistanceTraversalNodekIOS< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename Shape::S > &tf1, const BVHModel< kIOS< typename Shape::S >> &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result) |
Initialize traversal node for distance computation between one shape and one mesh, specialized for kIOS type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
FCL_EXPORT bool | initialize (ShapeMeshDistanceTraversalNodekIOS< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename Shape::S > &tf1, const BVHModel< kIOS< typename Shape::S >> &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result) |
Initialize traversal node for distance computation between one shape and one mesh, specialized for kIOS type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (ShapeMeshDistanceTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename Shape::S > &tf1, const BVHModel< OBBRSS< typename Shape::S >> &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result) |
Initialize traversal node for distance computation between one shape and one mesh, specialized for OBBRSS type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
FCL_EXPORT bool | initialize (ShapeMeshDistanceTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename Shape::S > &tf1, const BVHModel< OBBRSS< typename Shape::S >> &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result) |
Initialize traversal node for distance computation between one shape and one mesh, specialized for OBBRSS type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (ShapeMeshDistanceTraversalNodeRSS< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename Shape::S > &tf1, const BVHModel< RSS< typename Shape::S >> &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result) |
Initialize traversal node for distance computation between one shape and one mesh, specialized for RSS type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
FCL_EXPORT bool | initialize (ShapeMeshDistanceTraversalNodeRSS< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename Shape::S > &tf1, const BVHModel< RSS< typename Shape::S >> &model2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result) |
Initialize traversal node for distance computation between one shape and one mesh, specialized for RSS type. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (ShapeOcTreeCollisionTraversalNode< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename Shape::S > &tf1, const OcTree< typename Shape::S > &model2, const Transform3< typename Shape::S > &tf2, const OcTreeSolver< NarrowPhaseSolver > *otsolver, const CollisionRequest< typename Shape::S > &request, CollisionResult< typename Shape::S > &result) |
Initialize traversal node for collision between one shape and one octree, given current object transform. More... | |
template<typename Shape , typename NarrowPhaseSolver > | |
bool | initialize (ShapeOcTreeDistanceTraversalNode< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename Shape::S > &tf1, const OcTree< typename Shape::S > &model2, const Transform3< typename Shape::S > &tf2, const OcTreeSolver< NarrowPhaseSolver > *otsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result) |
Initialize traversal node for distance between one shape and one octree, given current object transform. More... | |
template int | intersectRectQuad2 (double h[2], double p[8], double ret[16]) |
template<typename S > | |
FCL_EXPORT int | intersectRectQuad2 (S h[2], S p[8], S ret[16]) |
template<typename S > | |
int | intersectRectQuad2 (S h[2], S p[8], S ret[16]) |
template<typename T > | |
bool | IsElementInSet (const std::unordered_set< T * > &S, const T *element) |
template void | lineClosestApproach (const Vector3< double > &pa, const Vector3< double > &ua, const Vector3< double > &pb, const Vector3< double > &ub, double *alpha, double *beta) |
template<typename S > | |
FCL_EXPORT void | lineClosestApproach (const Vector3< S > &pa, const Vector3< S > &ua, const Vector3< S > &pb, const Vector3< S > &ub, S *alpha, S *beta) |
template<typename S > | |
void | lineClosestApproach (const Vector3< S > &pa, const Vector3< S > &ua, const Vector3< S > &pb, const Vector3< S > &ub, S *alpha, S *beta) |
template void | lineSegmentPointClosestToPoint (const Vector3< double > &p, const Vector3< double > &s1, const Vector3< double > &s2, Vector3< double > &sp) |
template<typename S > | |
FCL_EXPORT void | lineSegmentPointClosestToPoint (const Vector3< S > &p, const Vector3< S > &s1, const Vector3< S > &s2, Vector3< S > &sp) |
template<typename S > | |
void | lineSegmentPointClosestToPoint (const Vector3< S > &p, const Vector3< S > &s1, const Vector3< S > &s2, Vector3< S > &sp) |
template<typename T > | |
void | MapFeature1ToFeature2 (const ccd_list_t *feature1_list, const ccd_list_t *feature2_list, std::function< bool(const T *, const T *)> cmp_feature, std::unordered_set< T * > *feature1, std::unordered_set< T * > *feature2, std::unordered_map< T *, T * > *map_feature1_to_feature2) |
template<typename S > | |
FCL_EXPORT S | maximumDistance_mesh (const Vector3< S > *const ps, const Vector3< S > *const ps2, Triangle *ts, unsigned int *indices, int n, const Vector3< S > &query) |
template double | maximumDistance_mesh (const Vector3d *const ps, const Vector3d *const ps2, Triangle *ts, unsigned int *indices, int n, const Vector3d &query) |
template<typename S > | |
FCL_EXPORT S | maximumDistance_pointcloud (const Vector3< S > *const ps, const Vector3< S > *const ps2, unsigned int *indices, int n, const Vector3< S > &query) |
template double | maximumDistance_pointcloud (const Vector3d *const ps, const Vector3d *const ps2, unsigned int *indices, int n, const Vector3d &query) |
template<typename BV > | |
FCL_EXPORT void | meshCollisionOrientedNodeLeafTesting (int b1, int b2, const BVHModel< BV > *model1, const BVHModel< BV > *model2, Vector3< typename BV::S > *vertices1, Vector3< typename BV::S > *vertices2, Triangle *tri_indices1, Triangle *tri_indices2, const Matrix3< typename BV::S > &R, const Vector3< typename BV::S > &T, const Transform3< typename BV::S > &tf1, const Transform3< typename BV::S > &tf2, bool enable_statistics, typename BV::S cost_density, int &num_leaf_tests, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result) |
template<typename BV > | |
void | meshCollisionOrientedNodeLeafTesting (int b1, int b2, const BVHModel< BV > *model1, const BVHModel< BV > *model2, Vector3< typename BV::S > *vertices1, Vector3< typename BV::S > *vertices2, Triangle *tri_indices1, Triangle *tri_indices2, const Matrix3< typename BV::S > &R, const Vector3< typename BV::S > &T, const Transform3< typename BV::S > &tf1, const Transform3< typename BV::S > &tf2, bool enable_statistics, typename BV::S cost_density, int &num_leaf_tests, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result) |
template<typename BV > | |
FCL_EXPORT void | meshCollisionOrientedNodeLeafTesting (int b1, int b2, const BVHModel< BV > *model1, const BVHModel< BV > *model2, Vector3< typename BV::S > *vertices1, Vector3< typename BV::S > *vertices2, Triangle *tri_indices1, Triangle *tri_indices2, const Transform3< typename BV::S > &tf, const Transform3< typename BV::S > &tf1, const Transform3< typename BV::S > &tf2, bool enable_statistics, typename BV::S cost_density, int &num_leaf_tests, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result) |
template<typename BV > | |
void | meshCollisionOrientedNodeLeafTesting (int b1, int b2, const BVHModel< BV > *model1, const BVHModel< BV > *model2, Vector3< typename BV::S > *vertices1, Vector3< typename BV::S > *vertices2, Triangle *tri_indices1, Triangle *tri_indices2, const Transform3< typename BV::S > &tf, const Transform3< typename BV::S > &tf1, const Transform3< typename BV::S > &tf2, bool enable_statistics, typename BV::S cost_density, int &num_leaf_tests, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result) |
template<typename BV > | |
FCL_EXPORT bool | meshConservativeAdvancementOrientedNodeCanStop (typename BV::S c, typename BV::S min_distance, typename BV::S abs_err, typename BV::S rel_err, typename BV::S w, const BVHModel< BV > *model1, const BVHModel< BV > *model2, const MotionBase< typename BV::S > *motion1, const MotionBase< typename BV::S > *motion2, std::vector< ConservativeAdvancementStackData< typename BV::S >> &stack, typename BV::S &delta_t) |
template<typename BV > | |
bool | meshConservativeAdvancementOrientedNodeCanStop (typename BV::S c, typename BV::S min_distance, typename BV::S abs_err, typename BV::S rel_err, typename BV::S w, const BVHModel< BV > *model1, const BVHModel< BV > *model2, const MotionBase< typename BV::S > *motion1, const MotionBase< typename BV::S > *motion2, std::vector< ConservativeAdvancementStackData< typename BV::S >> &stack, typename BV::S &delta_t) |
template<typename BV > | |
FCL_EXPORT void | meshConservativeAdvancementOrientedNodeLeafTesting (int b1, int b2, const BVHModel< BV > *model1, const BVHModel< BV > *model2, const Triangle *tri_indices1, const Triangle *tri_indices2, const Vector3< typename BV::S > *vertices1, const Vector3< typename BV::S > *vertices2, const Matrix3< typename BV::S > &R, const Vector3< typename BV::S > &T, const MotionBase< typename BV::S > *motion1, const MotionBase< typename BV::S > *motion2, bool enable_statistics, typename BV::S &min_distance, Vector3< typename BV::S > &p1, Vector3< typename BV::S > &p2, int &last_tri_id1, int &last_tri_id2, typename BV::S &delta_t, int &num_leaf_tests) |
template<typename BV > | |
void | meshConservativeAdvancementOrientedNodeLeafTesting (int b1, int b2, const BVHModel< BV > *model1, const BVHModel< BV > *model2, const Triangle *tri_indices1, const Triangle *tri_indices2, const Vector3< typename BV::S > *vertices1, const Vector3< typename BV::S > *vertices2, const Matrix3< typename BV::S > &R, const Vector3< typename BV::S > &T, const MotionBase< typename BV::S > *motion1, const MotionBase< typename BV::S > *motion2, bool enable_statistics, typename BV::S &min_distance, Vector3< typename BV::S > &p1, Vector3< typename BV::S > &p2, int &last_tri_id1, int &last_tri_id2, typename BV::S &delta_t, int &num_leaf_tests) |
template<typename BV > | |
FCL_EXPORT bool | meshConservativeAdvancementTraversalNodeCanStop (typename BV::S c, typename BV::S min_distance, typename BV::S abs_err, typename BV::S rel_err, typename BV::S w, const BVHModel< BV > *model1, const BVHModel< BV > *model2, const MotionBase< typename BV::S > *motion1, const MotionBase< typename BV::S > *motion2, std::vector< ConservativeAdvancementStackData< typename BV::S >> &stack, typename BV::S &delta_t) |
template<typename BV > | |
bool | meshConservativeAdvancementTraversalNodeCanStop (typename BV::S c, typename BV::S min_distance, typename BV::S abs_err, typename BV::S rel_err, typename BV::S w, const BVHModel< BV > *model1, const BVHModel< BV > *model2, const MotionBase< typename BV::S > *motion1, const MotionBase< typename BV::S > *motion2, std::vector< ConservativeAdvancementStackData< typename BV::S >> &stack, typename BV::S &delta_t) |
template<typename BV > | |
FCL_DEPRECATED_EXPORT void | meshDistanceOrientedNodeLeafTesting (int b1, int b2, const BVHModel< BV > *model1, const BVHModel< BV > *model2, Vector3< typename BV::S > *vertices1, Vector3< typename BV::S > *vertices2, Triangle *tri_indices1, Triangle *tri_indices2, const Matrix3< typename BV::S > &R, const Vector3< typename BV::S > &T, bool enable_statistics, int &num_leaf_tests, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename BV > | |
void | meshDistanceOrientedNodeLeafTesting (int b1, int b2, const BVHModel< BV > *model1, const BVHModel< BV > *model2, Vector3< typename BV::S > *vertices1, Vector3< typename BV::S > *vertices2, Triangle *tri_indices1, Triangle *tri_indices2, const Matrix3< typename BV::S > &R, const Vector3< typename BV::S > &T, bool enable_statistics, int &num_leaf_tests, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename BV > | |
FCL_EXPORT void | meshDistanceOrientedNodeLeafTesting (int b1, int b2, const BVHModel< BV > *model1, const BVHModel< BV > *model2, Vector3< typename BV::S > *vertices1, Vector3< typename BV::S > *vertices2, Triangle *tri_indices1, Triangle *tri_indices2, const Transform3< typename BV::S > &tf, bool enable_statistics, int &num_leaf_tests, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename BV > | |
void | meshDistanceOrientedNodeLeafTesting (int b1, int b2, const BVHModel< BV > *model1, const BVHModel< BV > *model2, Vector3< typename BV::S > *vertices1, Vector3< typename BV::S > *vertices2, Triangle *tri_indices1, Triangle *tri_indices2, const Transform3< typename BV::S > &tf, bool enable_statistics, int &num_leaf_tests, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename BV , typename Shape , typename NarrowPhaseSolver > | |
FCL_EXPORT void | meshShapeCollisionOrientedNodeLeafTesting (int b1, int b2, const BVHModel< BV > *model1, const Shape &model2, Vector3< typename BV::S > *vertices, Triangle *tri_indices, const Transform3< typename BV::S > &tf1, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, bool enable_statistics, typename BV::S cost_density, int &num_leaf_tests, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result) |
template<typename BV , typename Shape , typename NarrowPhaseSolver > | |
void | meshShapeCollisionOrientedNodeLeafTesting (int b1, int b2, const BVHModel< BV > *model1, const Shape &model2, Vector3< typename BV::S > *vertices, Triangle *tri_indices, const Transform3< typename BV::S > &tf1, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, bool enable_statistics, typename BV::S cost_density, int &num_leaf_tests, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result) |
template<typename BV , typename Shape > | |
bool | meshShapeConservativeAdvancementOrientedNodeCanStop (typename BV::S c, typename BV::S min_distance, typename BV::S abs_err, typename BV::S rel_err, typename BV::S w, const BVHModel< BV > *model1, const Shape &model2, const BV &model2_bv, const MotionBase< typename BV::S > *motion1, const MotionBase< typename BV::S > *motion2, std::vector< ConservativeAdvancementStackData< typename BV::S >> &stack, typename BV::S &delta_t) |
template<typename BV , typename Shape , typename NarrowPhaseSolver > | |
void | meshShapeConservativeAdvancementOrientedNodeLeafTesting (int b1, int, const BVHModel< BV > *model1, const Shape &model2, const BV &model2_bv, Vector3< typename BV::S > *vertices, Triangle *tri_indices, const Transform3< typename BV::S > &tf1, const Transform3< typename BV::S > &tf2, const MotionBase< typename BV::S > *motion1, const MotionBase< typename BV::S > *motion2, const NarrowPhaseSolver *nsolver, bool enable_statistics, typename BV::S &min_distance, Vector3< typename BV::S > &p1, Vector3< typename BV::S > &p2, int &last_tri_id, typename BV::S &delta_t, int &num_leaf_tests) |
template<typename BV , typename Shape , typename NarrowPhaseSolver > | |
void | meshShapeDistanceOrientedNodeLeafTesting (int b1, int, const BVHModel< BV > *model1, const Shape &model2, Vector3< typename BV::S > *vertices, Triangle *tri_indices, const Transform3< typename BV::S > &tf1, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, bool enable_statistics, int &num_leaf_tests, const DistanceRequest< typename BV::S > &, DistanceResult< typename BV::S > &result) |
template<typename S > | |
bool | nearestPointInBox (const Vector3< S > &size, const Vector3< S > &p_BQ, Vector3< S > *p_BN_ptr) |
template<typename S > | |
bool | nearestPointInCylinder (const S &height, const S &radius, const Vector3< S > &p_CQ, Vector3< S > *p_CN_ptr) |
template<typename BV > | |
bool | nodeBaseLess (NodeBase< BV > *a, NodeBase< BV > *b, int d) |
Compare two nodes accoording to the d-th dimension of node center. More... | |
template<typename OrientMeshShapeCollisionTraveralNode , typename BV , typename Shape , typename NarrowPhaseSolver > | |
std::size_t | orientedBVHShapeCollide (const CollisionGeometry< typename BV::S > *o1, const Transform3< typename BV::S > &tf1, const CollisionGeometry< typename BV::S > *o2, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result) |
template<typename OrientedMeshShapeDistanceTraversalNode , typename BV , typename Shape , typename NarrowPhaseSolver > | |
Shape::S | orientedBVHShapeDistance (const CollisionGeometry< typename Shape::S > *o1, const Transform3< typename Shape::S > &tf1, const CollisionGeometry< typename Shape::S > *o2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result) |
template<typename OrientedMeshCollisionTraversalNode , typename BV > | |
std::size_t | orientedMeshCollide (const CollisionGeometry< typename BV::S > *o1, const Transform3< typename BV::S > &tf1, const CollisionGeometry< typename BV::S > *o2, const Transform3< typename BV::S > &tf2, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result) |
template<typename OrientedMeshDistanceTraversalNode , typename BV > | |
BV::S | orientedMeshDistance (const CollisionGeometry< typename BV::S > *o1, const Transform3< typename BV::S > &tf1, const CollisionGeometry< typename BV::S > *o2, const Transform3< typename BV::S > &tf2, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename S > | |
bool | overlap (S a1, S a2, S b1, S b2) |
returns 1 if the intervals overlap, and 0 otherwise More... | |
template bool | planeHalfspaceIntersect (const Plane< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, Plane< double > &pl, Vector3< double > &p, Vector3< double > &d, double &penetration_depth, int &ret) |
template<typename S > | |
FCL_EXPORT bool | planeHalfspaceIntersect (const Plane< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, Plane< S > &pl, Vector3< S > &p, Vector3< S > &d, S &penetration_depth, int &ret) |
return whether plane collides with halfspace if the separation plane of the halfspace is parallel with the plane return code 1, if the plane's normal is the same with halfspace's normal and plane is inside halfspace, also return plane in pl return code 2, if the plane's normal is oppositie to the halfspace's normal and plane is inside halfspace, also return plane in pl plane is outside halfspace, collision-free if not parallel return the intersection ray, return code 3. ray origin is p and direction is d More... | |
template<typename S > | |
bool | planeHalfspaceIntersect (const Plane< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, Plane< S > &pl, Vector3< S > &p, Vector3< S > &d, S &penetration_depth, int &ret) |
return whether plane collides with halfspace if the separation plane of the halfspace is parallel with the plane return code 1, if the plane's normal is the same with halfspace's normal and plane is inside halfspace, also return plane in pl return code 2, if the plane's normal is oppositie to the halfspace's normal and plane is inside halfspace, also return plane in pl plane is outside halfspace, collision-free if not parallel return the intersection ray, return code 3. ray origin is p and direction is d More... | |
template bool | planeIntersect (const Plane< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts) |
template<typename S > | |
bool | planeIntersect (const Plane< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *) |
template<typename S > | |
FCL_EXPORT bool | planeIntersect (const Plane< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<> | |
FCL_EXPORT double | planeIntersectTolerance () |
template<typename S > | |
FCL_EXPORT S | planeIntersectTolerance () |
template<typename S > | |
S | planeIntersectTolerance () |
template bool | planeTriangleIntersect (const Plane< double > &s1, const Transform3< double > &tf1, const Vector3< double > &P1, const Vector3< double > &P2, const Vector3< double > &P3, const Transform3< double > &tf2, Vector3< double > *contact_points, double *penetration_depth, Vector3< double > *normal) |
template<typename S > | |
FCL_EXPORT bool | planeTriangleIntersect (const Plane< S > &s1, const Transform3< S > &tf1, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Transform3< S > &tf2, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal) |
template<typename S > | |
bool | planeTriangleIntersect (const Plane< S > &s1, const Transform3< S > &tf1, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Transform3< S > &tf2, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal) |
template bool | projectInTriangle (const Vector3< double > &p1, const Vector3< double > &p2, const Vector3< double > &p3, const Vector3< double > &normal, const Vector3< double > &p) |
template<typename S > | |
FCL_EXPORT bool | projectInTriangle (const Vector3< S > &p1, const Vector3< S > &p2, const Vector3< S > &p3, const Vector3< S > &normal, const Vector3< S > &p) |
Whether a point's projection is in a triangle. More... | |
template<typename S > | |
bool | projectInTriangle (const Vector3< S > &p1, const Vector3< S > &p2, const Vector3< S > &p3, const Vector3< S > &normal, const Vector3< S > &p) |
Whether a point's projection is in a triangle. More... | |
template void | propagateBVHFrontListCollisionRecurse (CollisionTraversalNodeBase< double > *node, BVHFrontList *front_list) |
template<typename S > | |
FCL_EXPORT void | propagateBVHFrontListCollisionRecurse (CollisionTraversalNodeBase< S > *node, BVHFrontList *front_list) |
Recurse function for front list propagation. More... | |
template double | segmentSqrDistance (const Vector3< double > &from, const Vector3< double > &to, const Vector3< double > &p, Vector3< double > &nearest) |
template<typename S > | |
FCL_EXPORT S | segmentSqrDistance (const Vector3< S > &from, const Vector3< S > &to, const Vector3< S > &p, Vector3< S > &nearest) |
the minimum distance from a point to a line More... | |
template<typename S > | |
S | segmentSqrDistance (const Vector3< S > &from, const Vector3< S > &to, const Vector3< S > &p, Vector3< S > &nearest) |
the minimum distance from a point to a line More... | |
template<typename BV > | |
size_t | select (const BV &query, const NodeBase< BV > &node1, const NodeBase< BV > &node2) |
select from node1 and node2 which is close to a given query bounding volume. 0 for node1 and 1 for node2 More... | |
template<typename BV > | |
size_t | select (const NodeBase< BV > &query, const NodeBase< BV > &node1, const NodeBase< BV > &node2) |
select from node1 and node2 which is close to a given query. 0 for node1 and 1 for node2 More... | |
template void | selfCollide (CollisionTraversalNodeBase< double > *node, BVHFrontList *front_list) |
template<typename S > | |
void | selfCollide (CollisionTraversalNodeBase< S > *node, BVHFrontList *front_list) |
self collision on collision traversal node; can use front list to accelerate More... | |
template<typename S > | |
FCL_EXPORT void | selfCollide (CollisionTraversalNodeBase< S > *node, BVHFrontList *front_list=nullptr) |
self collision on collision traversal node; can use front list to accelerate More... | |
template void | selfCollisionRecurse (CollisionTraversalNodeBase< double > *node, int b, BVHFrontList *front_list) |
template<typename S > | |
FCL_EXPORT void | selfCollisionRecurse (CollisionTraversalNodeBase< S > *node, int b, BVHFrontList *front_list) |
Recurse function for self collision. Make sure node is set correctly so that the first and second tree are the same. More... | |
template<typename S > | |
void | SetUpBoxToBox (const Transform3< S > &X_WF, void **o1, void **o2, ccd_t *ccd, fcl::Transform3< S > *X_FB1, fcl::Transform3< S > *X_FB2) |
template<typename BV , typename OrientedNode > | |
bool | setupMeshCollisionOrientedNode (OrientedNode &node, const BVHModel< BV > &model1, const Transform3< typename BV::S > &tf1, const BVHModel< BV > &model2, const Transform3< typename BV::S > &tf2, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result) |
template<typename BV , typename OrientedDistanceNode > | |
bool | setupMeshConservativeAdvancementOrientedDistanceNode (OrientedDistanceNode &node, const BVHModel< BV > &model1, const Transform3< typename BV::S > &tf1, const BVHModel< BV > &model2, const Transform3< typename BV::S > &tf2, typename BV::S w) |
template<typename BV , typename OrientedNode > | |
static bool | setupMeshDistanceOrientedNode (OrientedNode &node, const BVHModel< BV > &model1, const Transform3< typename BV::S > &tf1, const BVHModel< BV > &model2, const Transform3< typename BV::S > &tf2, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename BV , typename Shape , typename NarrowPhaseSolver , template< typename, typename > class OrientedNode> | |
bool | setupMeshShapeCollisionOrientedNode (OrientedNode< Shape, NarrowPhaseSolver > &node, const BVHModel< BV > &model1, const Transform3< typename BV::S > &tf1, const Shape &model2, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result) |
template<typename BV , typename Shape , typename NarrowPhaseSolver , template< typename, typename > class OrientedNode> | |
static bool | setupMeshShapeDistanceOrientedNode (OrientedNode< Shape, NarrowPhaseSolver > &node, const BVHModel< BV > &model1, const Transform3< typename BV::S > &tf1, const Shape &model2, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename Shape , typename BV , typename NarrowPhaseSolver , template< typename, typename > class OrientedNode> | |
static bool | setupShapeMeshCollisionOrientedNode (OrientedNode< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename BV::S > &tf1, const BVHModel< BV > &model2, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result) |
template<typename Shape , typename BV , typename NarrowPhaseSolver , template< typename, typename > class OrientedNode> | |
static bool | setupShapeMeshDistanceOrientedNode (OrientedNode< Shape, NarrowPhaseSolver > &node, const Shape &model1, const Transform3< typename BV::S > &tf1, const BVHModel< BV > &model2, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result) |
template<typename Shape , typename BV , typename NarrowPhaseSolver > | |
BV::S | ShapeBVHConservativeAdvancement (const CollisionGeometry< typename BV::S > *o1, const MotionBase< typename BV::S > *motion1, const CollisionGeometry< typename BV::S > *o2, const MotionBase< typename BV::S > *motion2, const NarrowPhaseSolver *nsolver, const ContinuousCollisionRequest< typename BV::S > &request, ContinuousCollisionResult< typename BV::S > &result) |
template<typename Shape1 , typename Shape2 , typename NarrowPhaseSolver > | |
Shape1::S | ShapeConservativeAdvancement (const CollisionGeometry< typename Shape1::S > *o1, const MotionBase< typename Shape1::S > *motion1, const CollisionGeometry< typename Shape1::S > *o2, const MotionBase< typename Shape1::S > *motion2, const NarrowPhaseSolver *nsolver, const ContinuousCollisionRequest< typename Shape1::S > &request, ContinuousCollisionResult< typename Shape1::S > &result) |
template<typename Shape1 , typename Shape2 , typename NarrowPhaseSolver > | |
std::size_t | ShapeShapeCollide (const CollisionGeometry< typename Shape1::S > *o1, const Transform3< typename Shape1::S > &tf1, const CollisionGeometry< typename Shape1::S > *o2, const Transform3< typename Shape1::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape1::S > &request, CollisionResult< typename Shape1::S > &result) |
template<typename Shape1 , typename Shape2 , typename NarrowPhaseSolver > | |
Shape1::S | ShapeShapeDistance (const CollisionGeometry< typename Shape1::S > *o1, const Transform3< typename Shape1::S > &tf1, const CollisionGeometry< typename Shape1::S > *o2, const Transform3< typename Shape1::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape1::S > &request, DistanceResult< typename Shape1::S > &result) |
template<typename S > | |
static void | shapeToGJK (const ShapeBase< S > &s, const Transform3< S > &tf, ccd_obj_t *o) |
template FCL_EXPORT bool | sphereBoxDistance (const Sphere< double > &sphere, const Transform3< double > &X_FS, const Box< double > &box, const Transform3< double > &X_FB, double *distance, Vector3< double > *p_FSb, Vector3< double > *p_FBs) |
template FCL_EXPORT bool | sphereBoxIntersect (const Sphere< double > &sphere, const Transform3< double > &X_FS, const Box< double > &box, const Transform3< double > &X_FB, std::vector< ContactPoint< double >> *contacts) |
template bool | sphereCapsuleDistance (const Sphere< double > &s1, const Transform3< double > &tf1, const Capsule< double > &s2, const Transform3< double > &tf2, double *dist, Vector3< double > *p1, Vector3< double > *p2) |
template<typename S > | |
FCL_EXPORT bool | sphereCapsuleDistance (const Sphere< S > &s1, const Transform3< S > &tf1, const Capsule< S > &s2, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2) |
template<typename S > | |
bool | sphereCapsuleDistance (const Sphere< S > &s1, const Transform3< S > &tf1, const Capsule< S > &s2, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2) |
template bool | sphereCapsuleIntersect (const Sphere< double > &s1, const Transform3< double > &tf1, const Capsule< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts) |
template<typename S > | |
FCL_EXPORT bool | sphereCapsuleIntersect (const Sphere< S > &s1, const Transform3< S > &tf1, const Capsule< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
bool | sphereCapsuleIntersect (const Sphere< S > &s1, const Transform3< S > &tf1, const Capsule< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template FCL_EXPORT bool | sphereCylinderDistance (const Sphere< double > &sphere, const Transform3< double > &X_FS, const Cylinder< double > &cylinder, const Transform3< double > &X_FC, double *distance, Vector3< double > *p_FSc, Vector3< double > *p_FCs) |
template FCL_EXPORT bool | sphereCylinderIntersect (const Sphere< double > &sphere, const Transform3< double > &X_FS, const Cylinder< double > &cylinder, const Transform3< double > &X_FC, std::vector< ContactPoint< double >> *contacts) |
template bool | sphereHalfspaceIntersect (const Sphere< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts) |
template<typename S > | |
FCL_EXPORT bool | sphereHalfspaceIntersect (const Sphere< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
bool | sphereHalfspaceIntersect (const Sphere< S > &s1, const Transform3< S > &tf1, const Halfspace< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template bool | spherePlaneIntersect (const Sphere< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts) |
template<typename S > | |
FCL_EXPORT bool | spherePlaneIntersect (const Sphere< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
bool | spherePlaneIntersect (const Sphere< S > &s1, const Transform3< S > &tf1, const Plane< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template FCL_EXPORT bool | sphereSphereDistance (const Sphere< double > &s1, const Transform3< double > &tf1, const Sphere< double > &s2, const Transform3< double > &tf2, double *dist, Vector3< double > *p1, Vector3< double > *p2) |
template<typename S > | |
bool | sphereSphereDistance (const Sphere< S > &s1, const Transform3< S > &tf1, const Sphere< S > &s2, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2) |
template<typename S > | |
FCL_EXPORT bool | sphereSphereDistance (const Sphere< S > &s1, const Transform3< S > &tf1, const Sphere< S > &s2, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2) |
template FCL_EXPORT bool | sphereSphereIntersect (const Sphere< double > &s1, const Transform3< double > &tf1, const Sphere< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts) |
template<typename S > | |
bool | sphereSphereIntersect (const Sphere< S > &s1, const Transform3< S > &tf1, const Sphere< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
FCL_EXPORT bool | sphereSphereIntersect (const Sphere< S > &s1, const Transform3< S > &tf1, const Sphere< S > &s2, const Transform3< S > &tf2, std::vector< ContactPoint< S >> *contacts) |
template<typename S > | |
static void | sphereToGJK (const Sphere< S > &s, const Transform3< S > &tf, ccd_sphere_t *sph) |
template bool | sphereTriangleDistance (const Sphere< double > &sp, const Transform3< double > &tf, const Vector3< double > &P1, const Vector3< double > &P2, const Vector3< double > &P3, double *dist) |
template bool | sphereTriangleDistance (const Sphere< double > &sp, const Transform3< double > &tf, const Vector3< double > &P1, const Vector3< double > &P2, const Vector3< double > &P3, double *dist, Vector3< double > *p1, Vector3< double > *p2) |
template bool | sphereTriangleDistance (const Sphere< double > &sp, const Transform3< double > &tf1, const Vector3< double > &P1, const Vector3< double > &P2, const Vector3< double > &P3, const Transform3< double > &tf2, double *dist, Vector3< double > *p1, Vector3< double > *p2) |
template<typename S > | |
FCL_EXPORT bool | sphereTriangleDistance (const Sphere< S > &sp, const Transform3< S > &tf, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, S *dist) |
template<typename S > | |
bool | sphereTriangleDistance (const Sphere< S > &sp, const Transform3< S > &tf, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, S *dist) |
template<typename S > | |
FCL_EXPORT bool | sphereTriangleDistance (const Sphere< S > &sp, const Transform3< S > &tf, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, S *dist, Vector3< S > *p1, Vector3< S > *p2) |
template<typename S > | |
bool | sphereTriangleDistance (const Sphere< S > &sp, const Transform3< S > &tf, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, S *dist, Vector3< S > *p1, Vector3< S > *p2) |
template<typename S > | |
FCL_EXPORT bool | sphereTriangleDistance (const Sphere< S > &sp, const Transform3< S > &tf1, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2) |
template<typename S > | |
bool | sphereTriangleDistance (const Sphere< S > &sp, const Transform3< S > &tf1, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Transform3< S > &tf2, S *dist, Vector3< S > *p1, Vector3< S > *p2) |
template bool | sphereTriangleIntersect (const Sphere< double > &s, const Transform3< double > &tf, const Vector3< double > &P1, const Vector3< double > &P2, const Vector3< double > &P3, Vector3< double > *contact_points, double *penetration_depth, Vector3< double > *normal_) |
template<typename S > | |
FCL_EXPORT bool | sphereTriangleIntersect (const Sphere< S > &s, const Transform3< S > &tf, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal_) |
template<typename S > | |
bool | sphereTriangleIntersect (const Sphere< S > &s, const Transform3< S > &tf, const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, Vector3< S > *contact_points, S *penetration_depth, Vector3< S > *normal_) |
static void | supportBox (const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v) |
static void | supportCap (const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v) |
static void | supportCone (const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v) |
template<typename S > | |
static void | supportConvex (const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v) |
static void | supportCyl (const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v) |
static void | supportEllipsoid (const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v) |
static void | supportSphere (const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v) |
static void | supportTriangle (const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v) |
TEST_F (ExtractClosestPoint, ExtractFrom1Simplex) | |
TEST_F (ExtractClosestPoint, ExtractFrom1SimplexSupport) | |
TEST_F (ExtractClosestPoint, ExtractFrom2Simplex) | |
TEST_F (ExtractClosestPoint, ExtractFrom2SimplexDegenerate) | |
TEST_F (ExtractClosestPoint, ExtractFrom2SimplexSupport) | |
TEST_F (ExtractClosestPoint, ExtractFrom3SimplesDegenerateCoincident) | |
TEST_F (ExtractClosestPoint, ExtractFrom3SimplesDegenerateColinear) | |
TEST_F (ExtractClosestPoint, ExtractFrom3Simplex) | |
template<typename S > | |
void | TestBoxes () |
template<typename S > | |
void | TestBoxesInFrameF (const Transform3< S > &X_WF) |
template<typename S > | |
void | TestCollidingSphereGJKSignedDistance () |
template<typename S > | |
void | TestNonCollidingSphereGJKSignedDistance () |
template<typename S > | |
void | TestSimplexToPolytope3 () |
template<typename S > | |
void | TestSimplexToPolytope3InGivenFrame (const Transform3< S > &X_WF) |
template<typename S > | |
void | TestSphereToSphereGJKSignedDistance (S radius1, S radius2, const Vector3< S > &p_F1, const Vector3< S > &p_F2, S solver_tolerance, S test_tol, S min_distance_expected) |
std::array< Vector3< ccd_real_t >, 4 > | TetrahedronColinearVertices () |
std::array< Vector3< ccd_real_t >, 4 > | TetrahedronSmallFaceVertices () |
template<typename Shape1 , typename Shape2 , typename Solver , typename S > | |
void | ThrowDetailedConfiguration (const Shape1 &s1, const Transform3< S > &X_FS1, const Shape2 &s2, const Transform3< S > &X_FS2, const Solver &solver, const std::exception &e) |
FCL_EXPORT void | ThrowFailedAtThisConfiguration (const std::string &message, const char *func, const char *file, int line) |
template<typename S > | |
ccd_vec3_t | ToCcdVec3 (const Eigen::Ref< const Vector3< S >> &v) |
template<typename S > | |
Vector3< S > | ToEigenVector (const ccd_vec3_t &v) |
bool | triangle_area_is_zero (const Vector3d &a, const Vector3d &b, const Vector3d &c) |
bool | TriangleMatch (const ccd_pt_face_t *f1, const ccd_pt_face_t *f2, const std::unordered_map< ccd_pt_edge_t *, ccd_pt_edge_t * > &map_e1_to_e2) |
template<typename S > | |
FCL_EXPORT void * | triCreateGJKObject (const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3) |
template<typename S > | |
void * | triCreateGJKObject (const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3) |
template<typename S > | |
FCL_EXPORT void * | triCreateGJKObject (const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Transform3< S > &tf) |
template<typename S > | |
void * | triCreateGJKObject (const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Transform3< S > &tf) |
template void * | triCreateGJKObject (const Vector3d &P1, const Vector3d &P2, const Vector3d &P3) |
template void * | triCreateGJKObject (const Vector3d &P1, const Vector3d &P2, const Vector3d &P3, const Transform3d &tf) |
void | triDeleteGJKObject (void *o_) |
GJKCenterFunction | triGetCenterFunction () |
GJKSupportFunction | triGetSupportFunction () |
initialize GJK Triangle More... | |
FCL_EXPORT void | updateFrontList (BVHFrontList *front_list, int b1, int b2) |
Add new front node into the front list. More... | |
template<typename Shape > | |
::testing::AssertionResult | ValidateRepresentation (const Shape &shape, const std::string &code_string) |
bool | VertexPositionCoincide (const ccd_pt_vertex_t *v1, const ccd_pt_vertex_t *v2, ccd_real_t tol) |
template<typename S > | |
void | WriteCommaSeparated (std::stringstream *sstream, const Transform3< S > &p) |
template<typename S > | |
Vector3< S > | z_axis (const Transform3< S > &X_AB) |
using fcl::detail::BVHFrontList = typedef std::list<BVHFrontNode> |
BVH front list is a list of front nodes.
Definition at line 69 of file BVH_front.h.
using fcl::detail::CollisionTraversalNodeBased = typedef CollisionTraversalNodeBase<double> |
Definition at line 82 of file collision_traversal_node_base.h.
using fcl::detail::CollisionTraversalNodeBasef = typedef CollisionTraversalNodeBase<float> |
Definition at line 81 of file collision_traversal_node_base.h.
using fcl::detail::DistanceFn = typedef std::function<ccd_real_t ( const void*, const void*, const ccd_t*, ccd_vec3_t*, ccd_vec3_t*)> |
Definition at line 2622 of file gjk_libccd-inl.h.
using fcl::detail::EPAd = typedef EPA<double> |
using fcl::detail::EPAf = typedef EPA<float> |
using fcl::detail::GJKCenterFunction = typedef void (*)(const void* obj, ccd_vec3_t* c) |
Definition at line 72 of file gjk_libccd.h.
using fcl::detail::GJKd = typedef GJK<double> |
using fcl::detail::GJKf = typedef GJK<float> |
using fcl::detail::GJKSolver_indepd = typedef GJKSolver_indep<double> |
Definition at line 201 of file gjk_solver_indep.h.
using fcl::detail::GJKSolver_indepf = typedef GJKSolver_indep<float> |
Definition at line 200 of file gjk_solver_indep.h.
using fcl::detail::GJKSolver_libccdd = typedef GJKSolver_libccd<double> |
Definition at line 187 of file gjk_solver_libccd.h.
using fcl::detail::GJKSolver_libccdf = typedef GJKSolver_libccd<float> |
Definition at line 186 of file gjk_solver_libccd.h.
using fcl::detail::GJKSupportFunction = typedef void (*)(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) |
callback function used by GJK algorithm
Definition at line 71 of file gjk_libccd.h.
using fcl::detail::Intersectd = typedef Intersect<double> |
Definition at line 258 of file intersect.h.
using fcl::detail::Intersectf = typedef Intersect<float> |
Definition at line 257 of file intersect.h.
using fcl::detail::IntervalTreed = typedef IntervalTree<double> |
Definition at line 135 of file interval_tree.h.
using fcl::detail::IntervalTreef = typedef IntervalTree<float> |
Definition at line 134 of file interval_tree.h.
using fcl::detail::IntervalTreeNoded = typedef IntervalTreeNode<double> |
Definition at line 96 of file interval_tree_node.h.
using fcl::detail::IntervalTreeNodef = typedef IntervalTreeNode<float> |
Definition at line 95 of file interval_tree_node.h.
using fcl::detail::it_recursion_noded = typedef it_recursion_node<double> |
Definition at line 65 of file interval_tree.h.
using fcl::detail::it_recursion_nodef = typedef it_recursion_node<float> |
Definition at line 64 of file interval_tree.h.
using fcl::detail::MeshCollisionTraversalNodekIOSd = typedef MeshCollisionTraversalNodekIOS<double> |
Definition at line 197 of file mesh_collision_traversal_node.h.
using fcl::detail::MeshCollisionTraversalNodekIOSf = typedef MeshCollisionTraversalNodekIOS<float> |
Definition at line 196 of file mesh_collision_traversal_node.h.
using fcl::detail::MeshCollisionTraversalNodeOBBd = typedef MeshCollisionTraversalNodeOBB<double> |
Definition at line 123 of file mesh_collision_traversal_node.h.
using fcl::detail::MeshCollisionTraversalNodeOBBf = typedef MeshCollisionTraversalNodeOBB<float> |
Definition at line 122 of file mesh_collision_traversal_node.h.
using fcl::detail::MeshCollisionTraversalNodeOBBRSSd = typedef MeshCollisionTraversalNodeOBBRSS<double> |
Definition at line 230 of file mesh_collision_traversal_node.h.
using fcl::detail::MeshCollisionTraversalNodeOBBRSSf = typedef MeshCollisionTraversalNodeOBBRSS<float> |
Definition at line 229 of file mesh_collision_traversal_node.h.
using fcl::detail::MeshCollisionTraversalNodeRSSd = typedef MeshCollisionTraversalNodeRSS<double> |
Definition at line 165 of file mesh_collision_traversal_node.h.
using fcl::detail::MeshCollisionTraversalNodeRSSf = typedef MeshCollisionTraversalNodeRSS<float> |
Definition at line 164 of file mesh_collision_traversal_node.h.
using fcl::detail::MeshConservativeAdvancementTraversalNodeOBBRSSd = typedef MeshConservativeAdvancementTraversalNodeOBBRSS<double> |
Definition at line 199 of file mesh_conservative_advancement_traversal_node.h.
using fcl::detail::MeshConservativeAdvancementTraversalNodeOBBRSSf = typedef MeshConservativeAdvancementTraversalNodeOBBRSS<float> |
Definition at line 198 of file mesh_conservative_advancement_traversal_node.h.
using fcl::detail::MeshConservativeAdvancementTraversalNodeRSSd = typedef MeshConservativeAdvancementTraversalNodeRSS<double> |
Definition at line 148 of file mesh_conservative_advancement_traversal_node.h.
using fcl::detail::MeshConservativeAdvancementTraversalNodeRSSf = typedef MeshConservativeAdvancementTraversalNodeRSS<float> |
Definition at line 147 of file mesh_conservative_advancement_traversal_node.h.
using fcl::detail::MeshDistanceTraversalNodekIOSd = typedef MeshDistanceTraversalNodekIOS<double> |
Definition at line 162 of file mesh_distance_traversal_node.h.
using fcl::detail::MeshDistanceTraversalNodekIOSf = typedef MeshDistanceTraversalNodekIOS<float> |
Definition at line 161 of file mesh_distance_traversal_node.h.
using fcl::detail::MeshDistanceTraversalNodeOBBRSSd = typedef MeshDistanceTraversalNodeOBBRSS<double> |
Definition at line 203 of file mesh_distance_traversal_node.h.
using fcl::detail::MeshDistanceTraversalNodeOBBRSSf = typedef MeshDistanceTraversalNodeOBBRSS<float> |
Definition at line 202 of file mesh_distance_traversal_node.h.
using fcl::detail::MeshDistanceTraversalNodeRSSd = typedef MeshDistanceTraversalNodeRSS<double> |
Definition at line 121 of file mesh_distance_traversal_node.h.
using fcl::detail::MeshDistanceTraversalNodeRSSf = typedef MeshDistanceTraversalNodeRSS<float> |
Definition at line 120 of file mesh_distance_traversal_node.h.
using fcl::detail::MinkowskiDiffd = typedef MinkowskiDiff<double> |
Definition at line 96 of file minkowski_diff.h.
using fcl::detail::MinkowskiDifff = typedef MinkowskiDiff<float> |
Definition at line 95 of file minkowski_diff.h.
using fcl::detail::PolySolverd = typedef PolySolver<double> |
Definition at line 73 of file polysolver.h.
using fcl::detail::PolySolverf = typedef PolySolver<float> |
Definition at line 72 of file polysolver.h.
using fcl::detail::Projectd = typedef Project<double> |
using fcl::detail::Projectf = typedef Project<float> |
using fcl::detail::SimpleIntervald = typedef SimpleInterval<double> |
Definition at line 64 of file simple_interval.h.
using fcl::detail::SimpleIntervalf = typedef SimpleInterval<float> |
Definition at line 63 of file simple_interval.h.
using fcl::detail::SpatialHashd = typedef SpatialHash<double> |
Definition at line 67 of file spatial_hash.h.
using fcl::detail::SpatialHashf = typedef SpatialHash<float> |
Definition at line 66 of file spatial_hash.h.
using fcl::detail::TriangleDistanced = typedef TriangleDistance<double> |
Definition at line 107 of file triangle_distance.h.
using fcl::detail::TriangleDistancef = typedef TriangleDistance<float> |
Definition at line 106 of file triangle_distance.h.
using fcl::detail::Vector3d = typedef Vector3<double> |
Definition at line 47 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
Three types of split algorithms are provided in FCL as default.
Enumerator | |
---|---|
SPLIT_METHOD_MEAN | |
SPLIT_METHOD_MEDIAN | |
SPLIT_METHOD_BV_CENTER |
Definition at line 56 of file BV_splitter.h.
Definition at line 115 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
::testing::AssertionResult fcl::detail::are_same | ( | const Vector3d & | expected, |
const ccd_vec3_t & | tested, | ||
double | tolerance | ||
) |
Definition at line 68 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
template int fcl::detail::boxBox2 | ( | const Vector3< double > & | side1, |
const Transform3< double > & | tf1, | ||
const Vector3< double > & | side2, | ||
const Transform3< double > & | tf2, | ||
Vector3< double > & | normal, | ||
double * | depth, | ||
int * | return_code, | ||
int | maxc, | ||
std::vector< ContactPoint< double >> & | contacts | ||
) |
FCL_EXPORT int fcl::detail::boxBox2 | ( | const Vector3< S > & | side1, |
const Eigen::MatrixBase< DerivedA > & | R1, | ||
const Eigen::MatrixBase< DerivedB > & | T1, | ||
const Vector3< S > & | side2, | ||
const Eigen::MatrixBase< DerivedA > & | R2, | ||
const Eigen::MatrixBase< DerivedB > & | T2, | ||
Vector3< S > & | normal, | ||
S * | depth, | ||
int * | return_code, | ||
int | maxc, | ||
std::vector< ContactPoint< S >> & | contacts | ||
) |
Definition at line 248 of file box_box-inl.h.
int fcl::detail::boxBox2 | ( | const Vector3< S > & | side1, |
const Eigen::MatrixBase< DerivedA > & | R1, | ||
const Eigen::MatrixBase< DerivedB > & | T1, | ||
const Vector3< S > & | side2, | ||
const Eigen::MatrixBase< DerivedA > & | R2, | ||
const Eigen::MatrixBase< DerivedB > & | T2, | ||
Vector3< S > & | normal, | ||
S * | depth, | ||
int * | return_code, | ||
int | maxc, | ||
std::vector< ContactPoint< S >> & | contacts | ||
) |
Definition at line 248 of file box_box-inl.h.
FCL_EXPORT int fcl::detail::boxBox2 | ( | const Vector3< S > & | side1, |
const Transform3< S > & | tf1, | ||
const Vector3< S > & | side2, | ||
const Transform3< S > & | tf2, | ||
Vector3< S > & | normal, | ||
S * | depth, | ||
int * | return_code, | ||
int | maxc, | ||
std::vector< ContactPoint< S >> & | contacts | ||
) |
Definition at line 840 of file box_box-inl.h.
int fcl::detail::boxBox2 | ( | const Vector3< S > & | side1, |
const Transform3< S > & | tf1, | ||
const Vector3< S > & | side2, | ||
const Transform3< S > & | tf2, | ||
Vector3< S > & | normal, | ||
S * | depth, | ||
int * | return_code, | ||
int | maxc, | ||
std::vector< ContactPoint< S >> & | contacts | ||
) |
Definition at line 840 of file box_box-inl.h.
template bool fcl::detail::boxBoxIntersect | ( | const Box< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Box< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts_ | ||
) |
FCL_EXPORT bool fcl::detail::boxBoxIntersect | ( | const Box< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Box< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts_ | ||
) |
Definition at line 857 of file box_box-inl.h.
bool fcl::detail::boxBoxIntersect | ( | const Box< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Box< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts_ | ||
) |
Definition at line 857 of file box_box-inl.h.
template bool fcl::detail::boxHalfspaceIntersect | ( | const Box< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Halfspace< double > & | s2, | ||
const Transform3< double > & | tf2 | ||
) |
template bool fcl::detail::boxHalfspaceIntersect | ( | const Box< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Halfspace< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
FCL_EXPORT bool fcl::detail::boxHalfspaceIntersect | ( | const Box< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2 | ||
) |
box half space, a, b, c = +/- edge size n^T * (R(o + a v1 + b v2 + c v3) + T) <= d so (R^T n) (a v1 + b v2 + c v3) + n * T <= d check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c the max value of left side is d - n * T + |(R^T n) (a v1 + b v2 + c v3)|, check that is enough
Definition at line 226 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
bool fcl::detail::boxHalfspaceIntersect | ( | const Box< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2 | ||
) |
box half space, a, b, c = +/- edge size n^T * (R(o + a v1 + b v2 + c v3) + T) <= d so (R^T n) (a v1 + b v2 + c v3) + n * T <= d check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c the max value of left side is d - n * T + |(R^T n) (a v1 + b v2 + c v3)|, check that is enough
Definition at line 226 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
FCL_EXPORT bool fcl::detail::boxHalfspaceIntersect | ( | const Box< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
find deepest point
compute the contact point from the deepest point
Definition at line 244 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
bool fcl::detail::boxHalfspaceIntersect | ( | const Box< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
find deepest point
compute the contact point from the deepest point
Definition at line 244 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
template bool fcl::detail::boxPlaneIntersect | ( | const Box< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Plane< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
FCL_EXPORT bool fcl::detail::boxPlaneIntersect | ( | const Box< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
box half space, a, b, c = +/- edge size n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d so (R^T n) (a v1 + b v2 + c v3) + n * T ~ d check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c and <=0 for some a, b, c so need to check whether |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)|, the reason is as follows: (R^T n) (a v1 + b v2 + c v3) can get |(R^T n) (a v1 + b v2 + c v3)| for one a, b, c. if |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)| then can get both positive and negative value on the right side.
Definition at line 197 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
bool fcl::detail::boxPlaneIntersect | ( | const Box< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
box half space, a, b, c = +/- edge size n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d so (R^T n) (a v1 + b v2 + c v3) + n * T ~ d check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c and <=0 for some a, b, c so need to check whether |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)|, the reason is as follows: (R^T n) (a v1 + b v2 + c v3) can get |(R^T n) (a v1 + b v2 + c v3)| for one a, b, c. if |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)| then can get both positive and negative value on the right side.
Definition at line 197 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
|
static |
Definition at line 2278 of file gjk_libccd-inl.h.
std::size_t fcl::detail::BVHCollide | ( | const CollisionGeometry< typename BV::S > * | o1, |
const Transform3< typename BV::S > & | tf1, | ||
const CollisionGeometry< typename BV::S > * | o2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result | ||
) |
Definition at line 558 of file collision_func_matrix-inl.h.
std::size_t fcl::detail::BVHCollide | ( | const CollisionGeometry< typename BV::S > * | o1, |
const Transform3< typename BV::S > & | tf1, | ||
const CollisionGeometry< typename BV::S > * | o2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result | ||
) |
Definition at line 648 of file collision_func_matrix-inl.h.
BV::S fcl::detail::BVHConservativeAdvancement | ( | const CollisionGeometry< typename BV::S > * | o1, |
const MotionBase< typename BV::S > * | motion1, | ||
const CollisionGeometry< typename BV::S > * | o2, | ||
const MotionBase< typename BV::S > * | motion2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const ContinuousCollisionRequest< typename BV::S > & | request, | ||
ContinuousCollisionResult< typename BV::S > & | result | ||
) |
Definition at line 693 of file conservative_advancement_func_matrix-inl.h.
BV::S fcl::detail::BVHDistance | ( | const CollisionGeometry< typename BV::S > * | o1, |
const Transform3< typename BV::S > & | tf1, | ||
const CollisionGeometry< typename BV::S > * | o2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result | ||
) |
Definition at line 374 of file distance_func_matrix-inl.h.
BV::S fcl::detail::BVHDistance | ( | const CollisionGeometry< typename BV::S > * | o1, |
const Transform3< typename BV::S > & | tf1, | ||
const CollisionGeometry< typename BV::S > * | o2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result | ||
) |
Definition at line 462 of file distance_func_matrix-inl.h.
BV::S fcl::detail::BVHShapeConservativeAdvancement | ( | const CollisionGeometry< typename BV::S > * | o1, |
const MotionBase< typename BV::S > * | motion1, | ||
const CollisionGeometry< typename BV::S > * | o2, | ||
const MotionBase< typename BV::S > * | motion2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const ContinuousCollisionRequest< typename BV::S > & | request, | ||
ContinuousCollisionResult< typename BV::S > & | result | ||
) |
Definition at line 758 of file conservative_advancement_func_matrix-inl.h.
template bool fcl::detail::capsuleCapsuleDistance | ( | const Capsule< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Capsule< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
double * | dist, | ||
Vector3d * | p1_res, | ||
Vector3d * | p2_res | ||
) |
FCL_EXPORT bool fcl::detail::capsuleCapsuleDistance | ( | const Capsule< S > & | s1, |
const Transform3< S > & | X_FC1, | ||
const Capsule< S > & | s2, | ||
const Transform3< S > & | X_FC2, | ||
S * | dist, | ||
Vector3< S > * | p_FW1, | ||
Vector3< S > * | p_FW2 | ||
) |
Computes the signed distance between two capsules s1
and s2
(with given poses X_FC1
and X_FC2
of the two capsules in a common frame F
). Further reports the witness points to that distance (p_FW1
and p_FW2
).
It is guaranteed that |p_FW1 - p_FW2| = |dist|
.
There are degenerate cases where there is no unique pair of witness points. If the center lines of the two capsules intersect (either once or multiple times when the are co-linear) then distance will still be reported (a negative value with the maximum magnitude possible between the two capsules) and an arbitrary pair of witness points from the set of valid pairs.
s1 | The first capsule. |
X_FC1 | The pose of the first capsule in a common frame F . |
s2 | The second capsule. |
X_FC2 | The pose of the second capsule in a common frame F . |
dist | The signed distance between the two capsules (negative indicates intersection). |
p_FW1 | The first witness point: a point on the surface of the first capsule measured and expressed in the common frame F . |
p_FW2 | The second witness point: a point on the surface of the second capsule measured and expressed in the common frame F . |
true
. S | The scalar type for computation. |
Definition at line 160 of file capsule_capsule-inl.h.
bool fcl::detail::capsuleCapsuleDistance | ( | const Capsule< S > & | s1, |
const Transform3< S > & | X_FC1, | ||
const Capsule< S > & | s2, | ||
const Transform3< S > & | X_FC2, | ||
S * | dist, | ||
Vector3< S > * | p_FW1, | ||
Vector3< S > * | p_FW2 | ||
) |
Computes the signed distance between two capsules s1
and s2
(with given poses X_FC1
and X_FC2
of the two capsules in a common frame F
). Further reports the witness points to that distance (p_FW1
and p_FW2
).
It is guaranteed that |p_FW1 - p_FW2| = |dist|
.
There are degenerate cases where there is no unique pair of witness points. If the center lines of the two capsules intersect (either once or multiple times when the are co-linear) then distance will still be reported (a negative value with the maximum magnitude possible between the two capsules) and an arbitrary pair of witness points from the set of valid pairs.
s1 | The first capsule. |
X_FC1 | The pose of the first capsule in a common frame F . |
s2 | The second capsule. |
X_FC2 | The pose of the second capsule in a common frame F . |
dist | The signed distance between the two capsules (negative indicates intersection). |
p_FW1 | The first witness point: a point on the surface of the first capsule measured and expressed in the common frame F . |
p_FW2 | The second witness point: a point on the surface of the second capsule measured and expressed in the common frame F . |
true
. S | The scalar type for computation. |
Definition at line 160 of file capsule_capsule-inl.h.
template bool fcl::detail::capsuleHalfspaceIntersect | ( | const Capsule< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Halfspace< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
FCL_EXPORT bool fcl::detail::capsuleHalfspaceIntersect | ( | const Capsule< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 315 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
bool fcl::detail::capsuleHalfspaceIntersect | ( | const Capsule< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 315 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
template bool fcl::detail::capsulePlaneIntersect | ( | const Capsule< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Plane< double > & | s2, | ||
const Transform3< double > & | tf2 | ||
) |
template bool fcl::detail::capsulePlaneIntersect | ( | const Capsule< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Plane< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
FCL_EXPORT bool fcl::detail::capsulePlaneIntersect | ( | const Capsule< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2 | ||
) |
Definition at line 269 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
bool fcl::detail::capsulePlaneIntersect | ( | const Capsule< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2 | ||
) |
Definition at line 269 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
FCL_EXPORT bool fcl::detail::capsulePlaneIntersect | ( | const Capsule< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 294 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
bool fcl::detail::capsulePlaneIntersect | ( | const Capsule< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 294 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
|
static |
Definition at line 2287 of file gjk_libccd-inl.h.
Vector3d fcl::detail::ccd_to_eigen | ( | const ccd_vec3_t & | vector | ) |
Definition at line 60 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
|
static |
Definition at line 2557 of file gjk_libccd-inl.h.
|
inlinestatic |
Definition at line 2550 of file gjk_libccd-inl.h.
|
static |
Definition at line 2566 of file gjk_libccd-inl.h.
void fcl::detail::CheckComputeVisiblePatch | ( | const Polytope & | polytope, |
ccd_pt_face_t & | face, | ||
const ccd_vec3_t & | new_vertex, | ||
const std::unordered_set< int > & | border_edge_indices_expected, | ||
const std::unordered_set< int > & | visible_face_indices_expected, | ||
const std::unordered_set< int > & | internal_edges_indices_expected | ||
) |
Definition at line 613 of file test_gjk_libccd-inl_epa.cpp.
void fcl::detail::CheckComputeVisiblePatchColinearNewVertex | ( | EquilateralTetrahedron & | tet, |
double | rho | ||
) |
Definition at line 712 of file test_gjk_libccd-inl_epa.cpp.
void fcl::detail::CheckComputeVisiblePatchCommon | ( | const Polytope & | polytope, |
const std::unordered_set< ccd_pt_edge_t * > & | border_edges, | ||
const std::unordered_set< ccd_pt_face_t * > & | visible_faces, | ||
const std::unordered_set< ccd_pt_edge_t * > & | internal_edges, | ||
const std::unordered_set< int > & | border_edge_indices_expected, | ||
const std::unordered_set< int > & | visible_face_indices_expected, | ||
const std::unordered_set< int > | internal_edges_indices_expected | ||
) |
Definition at line 558 of file test_gjk_libccd-inl_epa.cpp.
void fcl::detail::CheckComputeVisiblePatchRecursive | ( | const Polytope & | polytope, |
ccd_pt_face_t & | face, | ||
const std::vector< int > & | edge_indices, | ||
const ccd_vec3_t & | new_vertex, | ||
const std::unordered_set< int > & | border_edge_indices_expected, | ||
const std::unordered_set< int > & | visible_face_indices_expected, | ||
const std::unordered_set< int > & | internal_edges_indices_expected | ||
) |
Definition at line 586 of file test_gjk_libccd-inl_epa.cpp.
void fcl::detail::CheckTetrahedronFaceNormal | ( | const Tetrahedron & | p | ) |
Definition at line 193 of file test_gjk_libccd-inl_epa.cpp.
template double fcl::detail::clamp | ( | double | n, |
double | min, | ||
double | max | ||
) |
FCL_EXPORT S fcl::detail::clamp | ( | S | n, |
S | min, | ||
S | max | ||
) |
Definition at line 70 of file capsule_capsule-inl.h.
S fcl::detail::clamp | ( | S | n, |
S | min, | ||
S | max | ||
) |
Definition at line 70 of file capsule_capsule-inl.h.
FCL_EXPORT S fcl::detail::closestPtSegmentSegment | ( | const Vector3< S > & | p_FP1, |
const Vector3< S > & | p_FQ1, | ||
const Vector3< S > & | p_FP2, | ||
const Vector3< S > & | p_FQ2, | ||
S * | s, | ||
S * | t, | ||
Vector3< S > * | p_FC1, | ||
Vector3< S > * | p_FC2 | ||
) |
Computes the pair of closest points (p_FC1, p_FC2)
between two line segments given by point pairs (p_FP1, p_FQ1)
and (p_FP2, p_FQ2)`. If the lines are parallel, there may be no unique such point pair, in that case it computes one from the set of nearest pairs. As a by-product, it reports the squared distance between those points and the parameters (s
and t
) such that p_FC1 = p_FP1 + s * (p_FQ1 - p_FP1)
for segment one, and similarly for p_FC2
and t
, with 0 ≤ s,t ≤ 1
. All points are measured and expressed in a common frame F
.
p_FP1 | Segment 1's first point, P1. |
p_FQ1 | Segment 1's second point, Q1. |
p_FP2 | Segment 2's first point, P2. |
p_FQ2 | Segment 2's second point, Q2. |
s | The parameter relating C1 with P1 and Q1. |
t | The parameter relating C2 with P2 and Q2. |
p_FC1 | The point C1 on segment 1, nearest segment 2. |
p_FC2 | The point C2 on segment 2, nearest segment 1. |
S | The scalar type for computation. |
Definition at line 79 of file capsule_capsule-inl.h.
S fcl::detail::closestPtSegmentSegment | ( | const Vector3< S > & | p_FP1, |
const Vector3< S > & | p_FQ1, | ||
const Vector3< S > & | p_FP2, | ||
const Vector3< S > & | p_FQ2, | ||
S * | s, | ||
S * | t, | ||
Vector3< S > * | p_FC1, | ||
Vector3< S > * | p_FC2 | ||
) |
Computes the pair of closest points (p_FC1, p_FC2)
between two line segments given by point pairs (p_FP1, p_FQ1)
and (p_FP2, p_FQ2)`. If the lines are parallel, there may be no unique such point pair, in that case it computes one from the set of nearest pairs. As a by-product, it reports the squared distance between those points and the parameters (s
and t
) such that p_FC1 = p_FP1 + s * (p_FQ1 - p_FP1)
for segment one, and similarly for p_FC2
and t
, with 0 ≤ s,t ≤ 1
. All points are measured and expressed in a common frame F
.
p_FP1 | Segment 1's first point, P1. |
p_FQ1 | Segment 1's second point, Q1. |
p_FP2 | Segment 2's first point, P2. |
p_FQ2 | Segment 2's second point, Q2. |
s | The parameter relating C1 with P1 and Q1. |
t | The parameter relating C2 with P2 and Q2. |
p_FC1 | The point C1 on segment 1, nearest segment 2. |
p_FC2 | The point C2 on segment 2, nearest segment 1. |
S | The scalar type for computation. |
Definition at line 79 of file capsule_capsule-inl.h.
template double fcl::detail::closestPtSegmentSegment | ( | const Vector3d & | p_FP1, |
const Vector3d & | p_FQ1, | ||
const Vector3d & | p_FP2, | ||
const Vector3d & | p_FQ2, | ||
double * | s, | ||
double * | t, | ||
Vector3d * | p_FC1, | ||
Vector3d * | p_FC2 | ||
) |
template void fcl::detail::collide | ( | CollisionTraversalNodeBase< double > * | node, |
BVHFrontList * | front_list | ||
) |
void fcl::detail::collide | ( | CollisionTraversalNodeBase< S > * | node, |
BVHFrontList * | front_list | ||
) |
collision on collision traversal node; can use front list to accelerate
Definition at line 72 of file collision_node-inl.h.
FCL_EXPORT void fcl::detail::collide | ( | CollisionTraversalNodeBase< S > * | node, |
BVHFrontList * | front_list = nullptr |
||
) |
collision on collision traversal node; can use front list to accelerate
Definition at line 72 of file collision_node-inl.h.
template void fcl::detail::collide2 | ( | MeshCollisionTraversalNodeOBB< double > * | node, |
BVHFrontList * | front_list | ||
) |
void fcl::detail::collide2 | ( | MeshCollisionTraversalNodeOBB< S > * | node, |
BVHFrontList * | front_list | ||
) |
special collision on OBB traversal node
Definition at line 86 of file collision_node-inl.h.
FCL_EXPORT void fcl::detail::collide2 | ( | MeshCollisionTraversalNodeOBB< S > * | node, |
BVHFrontList * | front_list = nullptr |
||
) |
special collision on OBB traversal node
Definition at line 86 of file collision_node-inl.h.
template void fcl::detail::collide2 | ( | MeshCollisionTraversalNodeRSS< double > * | node, |
BVHFrontList * | front_list | ||
) |
void fcl::detail::collide2 | ( | MeshCollisionTraversalNodeRSS< S > * | node, |
BVHFrontList * | front_list | ||
) |
special collision on RSS traversal node
Definition at line 108 of file collision_node-inl.h.
FCL_EXPORT void fcl::detail::collide2 | ( | MeshCollisionTraversalNodeRSS< S > * | node, |
BVHFrontList * | front_list = nullptr |
||
) |
special collision on RSS traversal node
Definition at line 108 of file collision_node-inl.h.
template void fcl::detail::collisionRecurse | ( | CollisionTraversalNodeBase< double > * | node, |
int | b1, | ||
int | b2, | ||
BVHFrontList * | front_list | ||
) |
FCL_EXPORT void fcl::detail::collisionRecurse | ( | CollisionTraversalNodeBase< S > * | node, |
int | b1, | ||
int | b2, | ||
BVHFrontList * | front_list | ||
) |
Recurse function for collision.
Definition at line 84 of file traversal_recurse-inl.h.
template void fcl::detail::collisionRecurse | ( | MeshCollisionTraversalNodeOBB< double > * | node, |
int | b1, | ||
int | b2, | ||
const Matrix3< double > & | R, | ||
const Vector3< double > & | T, | ||
BVHFrontList * | front_list | ||
) |
FCL_EXPORT void fcl::detail::collisionRecurse | ( | MeshCollisionTraversalNodeOBB< S > * | node, |
int | b1, | ||
int | b2, | ||
const Matrix3< S > & | R, | ||
const Vector3< S > & | T, | ||
BVHFrontList * | front_list | ||
) |
Recurse function for collision, specialized for OBB type.
Definition at line 134 of file traversal_recurse-inl.h.
template void fcl::detail::collisionRecurse | ( | MeshCollisionTraversalNodeRSS< double > * | node, |
int | b1, | ||
int | b2, | ||
const Matrix3< double > & | R, | ||
const Vector3< double > & | T, | ||
BVHFrontList * | front_list | ||
) |
FCL_EXPORT void fcl::detail::collisionRecurse | ( | MeshCollisionTraversalNodeRSS< S > * | node, |
int | b1, | ||
int | b2, | ||
const Matrix3< S > & | R, | ||
const Vector3< S > & | T, | ||
BVHFrontList * | front_list | ||
) |
Recurse function for collision, specialized for RSS type.
Definition at line 220 of file traversal_recurse-inl.h.
void fcl::detail::CompareCcdVec3 | ( | const ccd_vec3_t & | v, |
const ccd_vec3_t & | v_expected, | ||
ccd_real_t | tol | ||
) |
Definition at line 1208 of file test_gjk_libccd-inl_epa.cpp.
void fcl::detail::ComparePolytope | ( | const ccd_pt_t * | polytope1, |
const ccd_pt_t * | polytope2, | ||
ccd_real_t | tol | ||
) |
Definition at line 921 of file test_gjk_libccd-inl_epa.cpp.
S fcl::detail::ComputeSphereSphereDistance | ( | S | radius1, |
S | radius2, | ||
const Vector3< S > & | p_F1, | ||
const Vector3< S > & | p_F2 | ||
) |
Definition at line 57 of file test_gjk_libccd-inl_signed_distance.cpp.
void fcl::detail::computeSplitValue_bvcenter | ( | const BV & | bv, |
S & | split_value | ||
) |
Definition at line 550 of file BV_splitter-inl.h.
void fcl::detail::computeSplitValue_mean | ( | const BV & | bv, |
Vector3< S > * | vertices, | ||
Triangle * | triangles, | ||
unsigned int * | primitive_indices, | ||
int | num_primitives, | ||
BVHModelType | type, | ||
const Vector3< S > & | split_vector, | ||
S & | split_value | ||
) |
Definition at line 558 of file BV_splitter-inl.h.
void fcl::detail::computeSplitValue_median | ( | const BV & | bv, |
Vector3< S > * | vertices, | ||
Triangle * | triangles, | ||
unsigned int * | primitive_indices, | ||
int | num_primitives, | ||
BVHModelType | type, | ||
const Vector3< S > & | split_vector, | ||
S & | split_value | ||
) |
Definition at line 603 of file BV_splitter-inl.h.
void fcl::detail::computeSplitVector | ( | const BV & | bv, |
Vector3< S > & | split_vector | ||
) |
Definition at line 523 of file BV_splitter-inl.h.
template bool fcl::detail::coneHalfspaceIntersect | ( | const Cone< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Halfspace< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
FCL_EXPORT bool fcl::detail::coneHalfspaceIntersect | ( | const Cone< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 432 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
bool fcl::detail::coneHalfspaceIntersect | ( | const Cone< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 432 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
template bool fcl::detail::conePlaneIntersect | ( | const Cone< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Plane< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
FCL_EXPORT bool fcl::detail::conePlaneIntersect | ( | const Cone< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 519 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
bool fcl::detail::conePlaneIntersect | ( | const Cone< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 519 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
|
static |
Definition at line 2305 of file gjk_libccd-inl.h.
bool fcl::detail::conservativeAdvancement | ( | const BVHModel< BV > & | o1, |
const MotionBase< typename BV::S > * | motion1, | ||
const BVHModel< BV > & | o2, | ||
const MotionBase< typename BV::S > * | motion2, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result, | ||
typename BV::S & | toc | ||
) |
Definition at line 75 of file conservative_advancement_func_matrix-inl.h.
bool fcl::detail::conservativeAdvancement | ( | const BVHModel< BV > & | o1, |
const MotionBase< typename BV::S > * | motion1, | ||
const Shape & | o2, | ||
const MotionBase< typename BV::S > * | motion2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result, | ||
typename BV::S & | toc | ||
) |
Definition at line 291 of file conservative_advancement_func_matrix-inl.h.
bool fcl::detail::conservativeAdvancement | ( | const BVHModel< OBBRSS< typename Shape::S >> & | o1, |
const MotionBase< typename Shape::S > * | motion1, | ||
const Shape & | o2, | ||
const MotionBase< typename Shape::S > * | motion2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result, | ||
typename Shape::S & | toc | ||
) |
Definition at line 444 of file conservative_advancement_func_matrix-inl.h.
bool fcl::detail::conservativeAdvancement | ( | const BVHModel< RSS< typename Shape::S >> & | o1, |
const MotionBase< typename Shape::S > * | motion1, | ||
const Shape & | o2, | ||
const MotionBase< typename Shape::S > * | motion2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result, | ||
typename Shape::S & | toc | ||
) |
Definition at line 429 of file conservative_advancement_func_matrix-inl.h.
bool fcl::detail::conservativeAdvancement | ( | const Shape & | o1, |
const MotionBase< typename BV::S > * | motion1, | ||
const BVHModel< BV > & | o2, | ||
const MotionBase< typename BV::S > * | motion2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result, | ||
typename BV::S & | toc | ||
) |
Definition at line 459 of file conservative_advancement_func_matrix-inl.h.
bool fcl::detail::conservativeAdvancement | ( | const Shape & | o1, |
const MotionBase< typename Shape::S > * | motion1, | ||
const BVHModel< OBBRSS< typename Shape::S >> & | o2, | ||
const MotionBase< typename Shape::S > * | motion2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result, | ||
typename Shape::S & | toc | ||
) |
Definition at line 642 of file conservative_advancement_func_matrix-inl.h.
bool fcl::detail::conservativeAdvancement | ( | const Shape & | o1, |
const MotionBase< typename Shape::S > * | motion1, | ||
const BVHModel< RSS< typename Shape::S >> & | o2, | ||
const MotionBase< typename Shape::S > * | motion2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result, | ||
typename Shape::S & | toc | ||
) |
Definition at line 627 of file conservative_advancement_func_matrix-inl.h.
bool fcl::detail::conservativeAdvancement | ( | const Shape1 & | o1, |
const MotionBase< typename Shape1::S > * | motion1, | ||
const Shape2 & | o2, | ||
const MotionBase< typename Shape1::S > * | motion2, | ||
const NarrowPhaseSolver * | solver, | ||
const CollisionRequest< typename Shape1::S > & | request, | ||
CollisionResult< typename Shape1::S > & | result, | ||
typename Shape1::S & | toc | ||
) |
Definition at line 222 of file conservative_advancement_func_matrix-inl.h.
bool fcl::detail::conservativeAdvancementMeshOriented | ( | const BVHModel< BV > & | o1, |
const MotionBase< typename BV::S > * | motion1, | ||
const BVHModel< BV > & | o2, | ||
const MotionBase< typename BV::S > * | motion2, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result, | ||
typename BV::S & | toc | ||
) |
Definition at line 150 of file conservative_advancement_func_matrix-inl.h.
bool fcl::detail::conservativeAdvancementMeshShapeOriented | ( | const BVHModel< BV > & | o1, |
const MotionBase< typename BV::S > * | motion1, | ||
const Shape & | o2, | ||
const MotionBase< typename BV::S > * | motion2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result, | ||
typename BV::S & | toc | ||
) |
Definition at line 361 of file conservative_advancement_func_matrix-inl.h.
bool fcl::detail::conservativeAdvancementShapeMeshOriented | ( | const Shape & | o1, |
const MotionBase< typename BV::S > * | motion1, | ||
const BVHModel< BV > & | o2, | ||
const MotionBase< typename BV::S > * | motion2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result, | ||
typename BV::S & | toc | ||
) |
Definition at line 529 of file conservative_advancement_func_matrix-inl.h.
FCL_EXPORT BV::S fcl::detail::continuousCollideBVHPolynomial | ( | const CollisionGeometry< typename BV::S > * | o1_, |
const TranslationMotion< typename BV::S > * | motion1, | ||
const CollisionGeometry< typename BV::S > * | o2_, | ||
const TranslationMotion< typename BV::S > * | motion2, | ||
const ContinuousCollisionRequest< typename BV::S > & | request, | ||
ContinuousCollisionResult< typename BV::S > & | result | ||
) |
Definition at line 179 of file continuous_collision-inl.h.
FCL_EXPORT NarrowPhaseSolver::S fcl::detail::continuousCollideConservativeAdvancement | ( | const CollisionGeometry< typename NarrowPhaseSolver::S > * | o1, |
const MotionBase< typename NarrowPhaseSolver::S > * | motion1, | ||
const CollisionGeometry< typename NarrowPhaseSolver::S > * | o2, | ||
const MotionBase< typename NarrowPhaseSolver::S > * | motion2, | ||
const NarrowPhaseSolver * | nsolver_, | ||
const ContinuousCollisionRequest< typename NarrowPhaseSolver::S > & | request, | ||
ContinuousCollisionResult< typename NarrowPhaseSolver::S > & | result | ||
) |
Definition at line 305 of file continuous_collision-inl.h.
template bool fcl::detail::convexHalfspaceIntersect | ( | const Convex< double > & | convex_C, |
const Transform3< double > & | X_FC, | ||
const Halfspace< double > & | half_space_H, | ||
const Transform3< double > & | X_FH, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
template bool fcl::detail::convexHalfspaceIntersect | ( | const Convex< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Halfspace< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
Vector3< double > * | contact_points, | ||
double * | penetration_depth, | ||
Vector3< double > * | normal | ||
) |
FCL_EXPORT bool fcl::detail::convexHalfspaceIntersect | ( | const Convex< S > & | convex_C, |
const Transform3< S > & | X_FC, | ||
const Halfspace< S > & | half_space_H, | ||
const Transform3< S > & | X_FH, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Reports whether a convex mesh and half space are intersecting. If contacts
is not nullptr
and the two geometries are intersecting, a new ContactPoint will be added to the data.
The two geometries are considered to be free of intersection if and only if all points in the Convex mesh lie strictly outside the half space. Lying on the boundary of the half space is considered intersecting.
The two geometries are each defined in their own frames, but we have transforms to a common frame F.
If the two geometries are intersecting and contacts
is not nullptr
, the new ContactPoint.normal will point in the opposite direction as the half space normal (expressed in Frame F) – it points into the half space. We define the point P to be a point of convex_C
that most deeply penetrates into the half space. It is not guaranteed to be unique. If it is not unique, it will be arbitrarily selected from the set of all such points. The ContactPoint.penetration_depth value is the depth of P. ContactPoint.pos is defined as the point halfway between P and the nearest point on the boundary of the half space, measured and expressed in F.
ContactPoint is documented to report contact position in the world frame W. This function will only truly satisfy that requirement if F = W. It is the responsibility of the caller to understand the semantics of F and confirm that it satisfies that requirement.
This makes use of the Drake monogram notation to describe quantities (particularly the poses of shapes).
convex_C | The convex mesh. Its vertex positions are measured and expressed in Frame C. | |
X_FC | The transform relating Frame C with the common frame F. | |
half_space_H | The half space. The position and orientation of its boundary plane are measured and expressed in Frame H. | |
X_FH | The transform relating Frame H with the common frame F. | |
[out] | contacts | The optional accumulator for data characterizing the intersection. |
true
if the two geometries are intersecting. S | The computational scalar. |
Definition at line 546 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
bool fcl::detail::convexHalfspaceIntersect | ( | const Convex< S > & | convex_C, |
const Transform3< S > & | X_FC, | ||
const Halfspace< S > & | half_space_H, | ||
const Transform3< S > & | X_FH, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Reports whether a convex mesh and half space are intersecting. If contacts
is not nullptr
and the two geometries are intersecting, a new ContactPoint will be added to the data.
The two geometries are considered to be free of intersection if and only if all points in the Convex mesh lie strictly outside the half space. Lying on the boundary of the half space is considered intersecting.
The two geometries are each defined in their own frames, but we have transforms to a common frame F.
If the two geometries are intersecting and contacts
is not nullptr
, the new ContactPoint.normal will point in the opposite direction as the half space normal (expressed in Frame F) – it points into the half space. We define the point P to be a point of convex_C
that most deeply penetrates into the half space. It is not guaranteed to be unique. If it is not unique, it will be arbitrarily selected from the set of all such points. The ContactPoint.penetration_depth value is the depth of P. ContactPoint.pos is defined as the point halfway between P and the nearest point on the boundary of the half space, measured and expressed in F.
ContactPoint is documented to report contact position in the world frame W. This function will only truly satisfy that requirement if F = W. It is the responsibility of the caller to understand the semantics of F and confirm that it satisfies that requirement.
This makes use of the Drake monogram notation to describe quantities (particularly the poses of shapes).
convex_C | The convex mesh. Its vertex positions are measured and expressed in Frame C. | |
X_FC | The transform relating Frame C with the common frame F. | |
half_space_H | The half space. The position and orientation of its boundary plane are measured and expressed in Frame H. | |
X_FH | The transform relating Frame H with the common frame F. | |
[out] | contacts | The optional accumulator for data characterizing the intersection. |
true
if the two geometries are intersecting. S | The computational scalar. |
Definition at line 546 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
FCL_EXPORT bool fcl::detail::convexHalfspaceIntersect | ( | const Convex< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
Vector3< S > * | contact_points, | ||
S * | penetration_depth, | ||
Vector3< S > * | normal | ||
) |
Definition at line 501 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
bool fcl::detail::convexHalfspaceIntersect | ( | const Convex< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
Vector3< S > * | contact_points, | ||
S * | penetration_depth, | ||
Vector3< S > * | normal | ||
) |
Definition at line 501 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
template bool fcl::detail::convexPlaneIntersect | ( | const Convex< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Plane< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
Vector3< double > * | contact_points, | ||
double * | penetration_depth, | ||
Vector3< double > * | normal | ||
) |
FCL_EXPORT bool fcl::detail::convexPlaneIntersect | ( | const Convex< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
Vector3< S > * | contact_points, | ||
S * | penetration_depth, | ||
Vector3< S > * | normal | ||
) |
Definition at line 642 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
bool fcl::detail::convexPlaneIntersect | ( | const Convex< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
Vector3< S > * | contact_points, | ||
S * | penetration_depth, | ||
Vector3< S > * | normal | ||
) |
Definition at line 642 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
|
static |
Definition at line 2332 of file gjk_libccd-inl.h.
template void fcl::detail::cullPoints2 | ( | int | n, |
double | p[], | ||
int | m, | ||
int | i0, | ||
int | iret[] | ||
) |
FCL_EXPORT void fcl::detail::cullPoints2 | ( | int | n, |
S | p[], | ||
int | m, | ||
int | i0, | ||
int | iret[] | ||
) |
Definition at line 173 of file box_box-inl.h.
void fcl::detail::cullPoints2 | ( | int | n, |
S | p[], | ||
int | m, | ||
int | i0, | ||
int | iret[] | ||
) |
Definition at line 173 of file box_box-inl.h.
template bool fcl::detail::cylinderHalfspaceIntersect | ( | const Cylinder< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Halfspace< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
FCL_EXPORT bool fcl::detail::cylinderHalfspaceIntersect | ( | const Cylinder< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 368 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
bool fcl::detail::cylinderHalfspaceIntersect | ( | const Cylinder< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 368 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
template bool fcl::detail::cylinderPlaneIntersect | ( | const Cylinder< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Plane< double > & | s2, | ||
const Transform3< double > & | tf2 | ||
) |
template bool fcl::detail::cylinderPlaneIntersect | ( | const Cylinder< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Plane< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
FCL_EXPORT bool fcl::detail::cylinderPlaneIntersect | ( | const Cylinder< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2 | ||
) |
cylinder-plane intersect n^T (R (r * cosa * v1 + r * sina * v2 + h * v3) + T) ~ d need one point to be positive and one to be negative (n^T * v3) * h + n * T -d + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) ~ 0 (n^T * v3) * h + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) + n * T - d ~ 0
Definition at line 392 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
bool fcl::detail::cylinderPlaneIntersect | ( | const Cylinder< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2 | ||
) |
cylinder-plane intersect n^T (R (r * cosa * v1 + r * sina * v2 + h * v3) + T) ~ d need one point to be positive and one to be negative (n^T * v3) * h + n * T -d + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) ~ 0 (n^T * v3) * h + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) + n * T - d ~ 0
Definition at line 392 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
FCL_EXPORT bool fcl::detail::cylinderPlaneIntersect | ( | const Cylinder< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 414 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
bool fcl::detail::cylinderPlaneIntersect | ( | const Cylinder< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 414 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
|
static |
Definition at line 2296 of file gjk_libccd-inl.h.
template void fcl::detail::distance | ( | DistanceTraversalNodeBase< double > * | node, |
BVHFrontList * | front_list, | ||
int | qsize | ||
) |
void fcl::detail::distance | ( | DistanceTraversalNodeBase< S > * | node, |
BVHFrontList * | front_list, | ||
int | qsize | ||
) |
distance computation on distance traversal node; can use front list to accelerate
Definition at line 137 of file collision_node-inl.h.
FCL_EXPORT void fcl::detail::distance | ( | DistanceTraversalNodeBase< S > * | node, |
BVHFrontList * | front_list = nullptr , |
||
int | qsize = 2 |
||
) |
distance computation on distance traversal node; can use front list to accelerate
Definition at line 137 of file collision_node-inl.h.
FCL_EXPORT void fcl::detail::distancePostprocessOrientedNode | ( | const BVHModel< BV > * | model1, |
const BVHModel< BV > * | model2, | ||
const Transform3< typename BV::S > & | tf1, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result | ||
) |
the points obtained by triDistance are not in world space: both are in object1's local coordinate system, so we need to convert them into the world space.
Definition at line 590 of file mesh_distance_traversal_node-inl.h.
void fcl::detail::distancePostprocessOrientedNode | ( | const BVHModel< BV > * | model1, |
const BVHModel< BV > * | model2, | ||
const Transform3< typename BV::S > & | tf1, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result | ||
) |
the points obtained by triDistance are not in world space: both are in object1's local coordinate system, so we need to convert them into the world space.
Definition at line 590 of file mesh_distance_traversal_node-inl.h.
FCL_EXPORT void fcl::detail::distancePreprocessOrientedNode | ( | const BVHModel< BV > * | model1, |
const BVHModel< BV > * | model2, | ||
const Vector3< typename BV::S > * | vertices1, | ||
Vector3< typename BV::S > * | vertices2, | ||
Triangle * | tri_indices1, | ||
Triangle * | tri_indices2, | ||
int | init_tri_id1, | ||
int | init_tri_id2, | ||
const Matrix3< typename BV::S > & | R, | ||
const Vector3< typename BV::S > & | T, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result | ||
) |
Definition at line 503 of file mesh_distance_traversal_node-inl.h.
void fcl::detail::distancePreprocessOrientedNode | ( | const BVHModel< BV > * | model1, |
const BVHModel< BV > * | model2, | ||
const Vector3< typename BV::S > * | vertices1, | ||
Vector3< typename BV::S > * | vertices2, | ||
Triangle * | tri_indices1, | ||
Triangle * | tri_indices2, | ||
int | init_tri_id1, | ||
int | init_tri_id2, | ||
const Matrix3< typename BV::S > & | R, | ||
const Vector3< typename BV::S > & | T, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result | ||
) |
Definition at line 503 of file mesh_distance_traversal_node-inl.h.
FCL_EXPORT void fcl::detail::distancePreprocessOrientedNode | ( | const BVHModel< BV > * | model1, |
const BVHModel< BV > * | model2, | ||
const Vector3< typename BV::S > * | vertices1, | ||
Vector3< typename BV::S > * | vertices2, | ||
Triangle * | tri_indices1, | ||
Triangle * | tri_indices2, | ||
int | init_tri_id1, | ||
int | init_tri_id2, | ||
const Transform3< typename BV::S > & | tf, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result | ||
) |
Definition at line 546 of file mesh_distance_traversal_node-inl.h.
void fcl::detail::distancePreprocessOrientedNode | ( | const BVHModel< BV > * | model1, |
const BVHModel< BV > * | model2, | ||
const Vector3< typename BV::S > * | vertices1, | ||
Vector3< typename BV::S > * | vertices2, | ||
Triangle * | tri_indices1, | ||
Triangle * | tri_indices2, | ||
int | init_tri_id1, | ||
int | init_tri_id2, | ||
const Transform3< typename BV::S > & | tf, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result | ||
) |
Definition at line 546 of file mesh_distance_traversal_node-inl.h.
void fcl::detail::distancePreprocessOrientedNode | ( | const BVHModel< BV > * | model1, |
Vector3< typename BV::S > * | vertices, | ||
Triangle * | tri_indices, | ||
int | init_tri_id, | ||
const Shape & | model2, | ||
const Transform3< typename BV::S > & | tf1, | ||
const Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename BV::S > & | , | ||
DistanceResult< typename BV::S > & | result | ||
) |
Definition at line 204 of file mesh_shape_distance_traversal_node-inl.h.
template void fcl::detail::distanceQueueRecurse | ( | DistanceTraversalNodeBase< double > * | node, |
int | b1, | ||
int | b2, | ||
BVHFrontList * | front_list, | ||
int | qsize | ||
) |
FCL_EXPORT void fcl::detail::distanceQueueRecurse | ( | DistanceTraversalNodeBase< S > * | node, |
int | b1, | ||
int | b2, | ||
BVHFrontList * | front_list, | ||
int | qsize | ||
) |
Recurse function for distance, using queue acceleration.
Definition at line 386 of file traversal_recurse-inl.h.
template void fcl::detail::distanceRecurse | ( | DistanceTraversalNodeBase< double > * | node, |
int | b1, | ||
int | b2, | ||
BVHFrontList * | front_list | ||
) |
FCL_EXPORT void fcl::detail::distanceRecurse | ( | DistanceTraversalNodeBase< S > * | node, |
int | b1, | ||
int | b2, | ||
BVHFrontList * | front_list | ||
) |
Recurse function for distance.
Definition at line 259 of file traversal_recurse-inl.h.
bool fcl::detail::EdgeMatch | ( | const ccd_pt_edge_t * | e1, |
const ccd_pt_edge_t * | e2, | ||
const std::unordered_map< ccd_pt_vertex_t *, ccd_pt_vertex_t * > & | map_v1_to_v2 | ||
) |
Definition at line 826 of file test_gjk_libccd-inl_epa.cpp.
ccd_vec3_t fcl::detail::eigen_to_ccd | ( | const Vector3d & | vector | ) |
Definition at line 52 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
template bool fcl::detail::ellipsoidHalfspaceIntersect | ( | const Ellipsoid< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Halfspace< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
FCL_EXPORT bool fcl::detail::ellipsoidHalfspaceIntersect | ( | const Ellipsoid< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 184 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
bool fcl::detail::ellipsoidHalfspaceIntersect | ( | const Ellipsoid< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 184 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
template bool fcl::detail::ellipsoidPlaneIntersect | ( | const Ellipsoid< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Plane< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
FCL_EXPORT bool fcl::detail::ellipsoidPlaneIntersect | ( | const Ellipsoid< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 153 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
bool fcl::detail::ellipsoidPlaneIntersect | ( | const Ellipsoid< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 153 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
|
static |
Definition at line 2322 of file gjk_libccd-inl.h.
std::array<fcl::Vector3<ccd_real_t>, 4> fcl::detail::EquilateralTetrahedronVertices | ( | ccd_real_t | bottom_center_x, |
ccd_real_t | bottom_center_y, | ||
ccd_real_t | bottom_center_z, | ||
ccd_real_t | edge_length | ||
) |
Definition at line 127 of file test_gjk_libccd-inl_epa.cpp.
const FCL_EXPORT Vector3<S> fcl::detail::getBVAxis | ( | const BV & | bv, |
int | i | ||
) |
Definition at line 485 of file mesh_conservative_advancement_traversal_node-inl.h.
const Vector3<typename BV::S> fcl::detail::getBVAxis | ( | const BV & | bv, |
int | i | ||
) |
Definition at line 485 of file mesh_conservative_advancement_traversal_node-inl.h.
FCL_EXPORT void fcl::detail::getExtentAndCenter_mesh | ( | const Vector3< S > *const | ps, |
const Vector3< S > *const | 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. The bounding volume axes are known.
Definition at line 294 of file geometry-inl.h.
template void fcl::detail::getExtentAndCenter_mesh | ( | const Vector3d *const | ps, |
const Vector3d *const | ps2, | ||
Triangle * | ts, | ||
unsigned int * | indices, | ||
int | n, | ||
const Matrix3d & | axis, | ||
Vector3d & | center, | ||
Vector3d & | extent | ||
) |
FCL_EXPORT void fcl::detail::getExtentAndCenter_pointcloud | ( | const Vector3< S > *const | ps, |
const Vector3< S > *const | ps2, | ||
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. The bounding volume axes are known.
Definition at line 229 of file geometry-inl.h.
template void fcl::detail::getExtentAndCenter_pointcloud | ( | const Vector3d *const | ps, |
const Vector3d *const | ps2, | ||
unsigned int * | indices, | ||
int | n, | ||
const Matrix3d & | axis, | ||
Vector3d & | center, | ||
Vector3d & | extent | ||
) |
Vector3<S> fcl::detail::getSupport | ( | const ShapeBase< S > * | shape, |
const Eigen::MatrixBase< Derived > & | dir | ||
) |
the support function for shape
Definition at line 67 of file minkowski_diff-inl.h.
FCL_EXPORT Vector3<S> fcl::detail::getSupport | ( | const ShapeBase< S > * | shape, |
const Eigen::MatrixBase< Derived > & | dir | ||
) |
the support function for shape
Definition at line 67 of file minkowski_diff-inl.h.
template bool fcl::detail::GJKCollide | ( | void * | obj1, |
ccd_support_fn | supp1, | ||
ccd_center_fn | cen1, | ||
void * | obj2, | ||
ccd_support_fn | supp2, | ||
ccd_center_fn | cen2, | ||
unsigned int | max_iterations, | ||
double | tolerance, | ||
Vector3d * | contact_points, | ||
double * | penetration_depth, | ||
Vector3d * | normal | ||
) |
FCL_EXPORT bool fcl::detail::GJKCollide | ( | void * | obj1, |
ccd_support_fn | supp1, | ||
ccd_center_fn | cen1, | ||
void * | obj2, | ||
ccd_support_fn | supp2, | ||
ccd_center_fn | cen2, | ||
unsigned int | max_iterations, | ||
S | tolerance, | ||
Vector3< S > * | contact_points, | ||
S * | penetration_depth, | ||
Vector3< S > * | normal | ||
) |
GJK collision algorithm.
libccd returns dir and pos in world space and dir is pointing from object 1 to object 2
Definition at line 2575 of file gjk_libccd-inl.h.
bool fcl::detail::GJKCollide | ( | void * | obj1, |
ccd_support_fn | supp1, | ||
ccd_center_fn | cen1, | ||
void * | obj2, | ||
ccd_support_fn | supp2, | ||
ccd_center_fn | cen2, | ||
unsigned int | max_iterations, | ||
S | tolerance, | ||
Vector3< S > * | contact_points, | ||
S * | penetration_depth, | ||
Vector3< S > * | normal | ||
) |
GJK collision algorithm.
libccd returns dir and pos in world space and dir is pointing from object 1 to object 2
Definition at line 2575 of file gjk_libccd-inl.h.
template bool fcl::detail::GJKDistance | ( | void * | obj1, |
ccd_support_fn | supp1, | ||
void * | obj2, | ||
ccd_support_fn | supp2, | ||
unsigned int | max_iterations, | ||
double | tolerance, | ||
double * | dist, | ||
Vector3d * | p1, | ||
Vector3d * | p2 | ||
) |
FCL_EXPORT bool fcl::detail::GJKDistance | ( | void * | obj1, |
ccd_support_fn | supp1, | ||
void * | obj2, | ||
ccd_support_fn | supp2, | ||
unsigned int | max_iterations, | ||
S | tolerance, | ||
S * | dist, | ||
Vector3< S > * | p1, | ||
Vector3< S > * | p2 | ||
) |
Compute the distance between two objects using GJK algorithm.
[in] | obj1 | A convex geometric object. |
[in] | supp1 | A function to compute the support of obj1 along some direction. |
[in] | obj2 | A convex geometric object. |
[in] | supp2 | A function to compute the support of obj2 along some direction. |
max_iterations | The maximal iterations before the GJK algorithm terminates. | |
[in] | tolerance | The tolerance used in GJK. When the change of distance is smaller than this tolerance, the algorithm terminates. |
[out] | dist | The distance between the objects. When the two objects are not colliding, this is the actual distance, a positive number. When the two objects are colliding, it is -1. |
[out] | p1 | The closest point on object 1 in the world frame. |
[out] | p2 | The closest point on object 2 in the world frame. |
is_separated | True if the objects are separated, false otherwise. |
Definition at line 2680 of file gjk_libccd-inl.h.
bool fcl::detail::GJKDistance | ( | void * | obj1, |
ccd_support_fn | supp1, | ||
void * | obj2, | ||
ccd_support_fn | supp2, | ||
unsigned int | max_iterations, | ||
S | tolerance, | ||
S * | dist, | ||
Vector3< S > * | p1, | ||
Vector3< S > * | p2 | ||
) |
Compute the distance between two objects using GJK algorithm.
[in] | obj1 | A convex geometric object. |
[in] | supp1 | A function to compute the support of obj1 along some direction. |
[in] | obj2 | A convex geometric object. |
[in] | supp2 | A function to compute the support of obj2 along some direction. |
max_iterations | The maximal iterations before the GJK algorithm terminates. | |
[in] | tolerance | The tolerance used in GJK. When the change of distance is smaller than this tolerance, the algorithm terminates. |
[out] | dist | The distance between the objects. When the two objects are not colliding, this is the actual distance, a positive number. When the two objects are colliding, it is -1. |
[out] | p1 | The closest point on object 1 in the world frame. |
[out] | p2 | The closest point on object 2 in the world frame. |
is_separated | True if the objects are separated, false otherwise. |
Definition at line 2680 of file gjk_libccd-inl.h.
bool fcl::detail::GJKDistanceImpl | ( | void * | obj1, |
ccd_support_fn | supp1, | ||
void * | obj2, | ||
ccd_support_fn | supp2, | ||
unsigned int | max_iterations, | ||
S | tolerance, | ||
detail::DistanceFn | distance_func, | ||
S * | res, | ||
Vector3< S > * | p1, | ||
Vector3< S > * | p2 | ||
) |
Compute the distance between two objects using GJK algorithm.
[in] | obj1 | A convex geometric object. |
[in] | supp1 | A function to compute the support of obj1 along some direction. |
[in] | obj2 | A convex geometric object. |
[in] | supp2 | A function to compute the support of obj2 along some direction. |
max_iterations | The maximal iterations before the GJK algorithm terminates. | |
[in] | tolerance | The tolerance used in GJK. When the change of distance is smaller than this tolerance, the algorithm terminates. |
[in] | distance_func | The actual function that computes the distance. Different functions should be passed in, depending on whether the user wants to compute a signed distance (with penetration depth) or not. |
[out] | res | The distance between the objects. When the two objects are not colliding, this is the actual distance, a positive number. When the two objects are colliding, it is a negative value. The actual meaning of the negative distance is defined by distance_func . |
[out] | p1 | The closest point on object 1 in the world frame. |
[out] | p2 | The closest point on object 2 in the world frame. |
is_separated | True if the objects are separated, false otherwise. |
Definition at line 2647 of file gjk_libccd-inl.h.
template bool fcl::detail::GJKSignedDistance | ( | void * | obj1, |
ccd_support_fn | supp1, | ||
void * | obj2, | ||
ccd_support_fn | supp2, | ||
unsigned int | max_iterations, | ||
double | tolerance, | ||
double * | dist, | ||
Vector3d * | p1, | ||
Vector3d * | p2 | ||
) |
FCL_EXPORT bool fcl::detail::GJKSignedDistance | ( | void * | obj1, |
ccd_support_fn | supp1, | ||
void * | obj2, | ||
ccd_support_fn | supp2, | ||
unsigned int | max_iterations, | ||
S | tolerance, | ||
S * | res, | ||
Vector3< S > * | p1, | ||
Vector3< S > * | p2 | ||
) |
Compute the signed distance between two objects using GJK and EPA algorithm.
[in] | obj1 | A convex geometric object. |
[in] | supp1 | A function to compute the support of obj1 along some direction. |
[in] | obj2 | A convex geometric object. |
[in] | supp2 | A function to compute the support of obj2 along some direction. |
[in] | max_iterations | The maximal iterations before the GJK algorithm terminates. |
[in] | tolerance | The tolerance used in GJK. When the change of distance is smaller than this tolerance, the algorithm terminates. |
[out] | dist | The distance between the objects. When the two objects are not colliding, this is the actual distance, a positive number. When the two objects are colliding, it is the negation of the penetration depth, a negative value. |
[out] | p1 | The closest point on object 1 in the world frame. |
[out] | p2 | The closest point on object 2 in the world frame. |
is_separated | True if the objects are separated, false otherwise. |
Compute the signed distance between two mesh objects. When the objects are separating, the signed distance is > 0. When the objects are touching or penetrating, the signed distance is <= 0.
tolerance. | When the objects are separating, the GJK algorithm terminates when the change of distance between iterations is smaller than this tolerance. Note that this does NOT necessarily mean that the computed distance is within tolerance to the actual distance. On the other hand, when the objects are penetrating, the EPA algorithm terminates when the difference between the upper bound and the lower bound of the penetration depth is smaller than tolerance . Hence the computed penetration depth is within tolerance to the true depth. |
Definition at line 2703 of file gjk_libccd-inl.h.
bool fcl::detail::GJKSignedDistance | ( | void * | obj1, |
ccd_support_fn | supp1, | ||
void * | obj2, | ||
ccd_support_fn | supp2, | ||
unsigned int | max_iterations, | ||
S | tolerance, | ||
S * | res, | ||
Vector3< S > * | p1, | ||
Vector3< S > * | p2 | ||
) |
Compute the signed distance between two mesh objects. When the objects are separating, the signed distance is > 0. When the objects are touching or penetrating, the signed distance is <= 0.
tolerance. | When the objects are separating, the GJK algorithm terminates when the change of distance between iterations is smaller than this tolerance. Note that this does NOT necessarily mean that the computed distance is within tolerance to the actual distance. On the other hand, when the objects are penetrating, the EPA algorithm terminates when the difference between the upper bound and the lower bound of the penetration depth is smaller than tolerance . Hence the computed penetration depth is within tolerance to the true depth. |
Definition at line 2703 of file gjk_libccd-inl.h.
fcl::detail::GTEST_TEST | ( | DegenerateGeometry | , |
CoincidentPoints | |||
) |
Definition at line 120 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
fcl::detail::GTEST_TEST | ( | DegenerateGeometry | , |
ZeroAreaTriangle | |||
) |
Definition at line 188 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
ComputeVisiblePatch_2FacesVisible | |||
) |
Definition at line 680 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
ComputeVisiblePatch_4FacesVisible | |||
) |
Definition at line 648 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
ComputeVisiblePatch_TopAndSideFacesVisible | |||
) |
Definition at line 666 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
ComputeVisiblePatch_TopFaceVisible | |||
) |
Definition at line 631 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
ComputeVisiblePatchColinearNewVertex | |||
) |
Definition at line 741 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
ComputeVisiblePatchSanityCheck | |||
) |
Definition at line 754 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
convert2SimplexToTetrahedron | |||
) |
Definition at line 1584 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
expandPolytope_error | |||
) |
Definition at line 1184 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
expandPolytope_hexagram_1visible_face | |||
) |
Definition at line 1090 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
expandPolytope_hexagram_4_visible_faces | |||
) |
Definition at line 1134 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
expandPolytope_tetrahedron1 | |||
) |
Definition at line 994 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
expandPolytope_tetrahedron_2visible_faces | |||
) |
Definition at line 1037 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
faceNormalPointingOutward | |||
) |
Definition at line 205 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
faceNormalPointingOutwardError | |||
) |
Definition at line 301 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
faceNormalPointingOutwardOriginNearFace1 | |||
) |
Definition at line 246 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
faceNormalPointingOutwardOriginNearFace2 | |||
) |
Definition at line 281 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
isOutsidePolytopeFace | |||
) |
Definition at line 397 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
isOutsidePolytopeFaceError | |||
) |
Definition at line 435 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
penEPAPosClosest_edge | |||
) |
Definition at line 1235 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
penEPAPosClosest_face | |||
) |
Definition at line 1269 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
penEPAPosClosest_vertex | |||
) |
Definition at line 1215 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
supportEPADirection | |||
) |
Definition at line 327 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJK_EPA | , |
supportEPADirectionError | |||
) |
Definition at line 385 of file test_gjk_libccd-inl_epa.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJKSignedDistance | , |
box_box | |||
) |
Definition at line 445 of file test_gjk_libccd-inl_signed_distance.cpp.
fcl::detail::GTEST_TEST | ( | FCL_GJKSignedDistance | , |
sphere_sphere | |||
) |
Definition at line 225 of file test_gjk_libccd-inl_signed_distance.cpp.
template bool fcl::detail::halfspaceIntersect | ( | const Halfspace< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Halfspace< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
Vector3< double > & | p, | ||
Vector3< double > & | d, | ||
Halfspace< double > & | s, | ||
double & | penetration_depth, | ||
int & | ret | ||
) |
FCL_EXPORT bool fcl::detail::halfspaceIntersect | ( | const Halfspace< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
Vector3< S > & | p, | ||
Vector3< S > & | d, | ||
Halfspace< S > & | s, | ||
S & | penetration_depth, | ||
int & | ret | ||
) |
return whether two halfspace intersect if the separation planes of the two halfspaces are parallel return code 1, if two halfspaces' normal are same and s1 is in s2, also return s1 in s; return code 2, if two halfspaces' normal are same and s2 is in s1, also return s2 in s; return code 3, if two halfspaces' normal are opposite and s1 and s2 are into each other; collision free, if two halfspaces' are separate; if the separation planes of the two halfspaces are not parallel, return intersection ray, return code 4. ray origin is p and direction is d collision free return code 0
Definition at line 691 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
bool fcl::detail::halfspaceIntersect | ( | const Halfspace< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
Vector3< S > & | p, | ||
Vector3< S > & | d, | ||
Halfspace< S > & | s, | ||
S & | penetration_depth, | ||
int & | ret | ||
) |
return whether two halfspace intersect if the separation planes of the two halfspaces are parallel return code 1, if two halfspaces' normal are same and s1 is in s2, also return s1 in s; return code 2, if two halfspaces' normal are same and s2 is in s1, also return s2 in s; return code 3, if two halfspaces' normal are opposite and s1 and s2 are into each other; collision free, if two halfspaces' are separate; if the separation planes of the two halfspaces are not parallel, return intersection ray, return code 4. ray origin is p and direction is d collision free return code 0
Definition at line 691 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
double fcl::detail::halfspaceIntersectTolerance | ( | ) |
Definition at line 48 of file narrowphase/detail/primitive_shape_algorithm/halfspace.cpp.
FCL_EXPORT S fcl::detail::halfspaceIntersectTolerance | ( | ) |
Definition at line 148 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
S fcl::detail::halfspaceIntersectTolerance | ( | ) |
Definition at line 148 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
template bool fcl::detail::halfspacePlaneIntersect | ( | const Halfspace< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Plane< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
Plane< double > & | pl, | ||
Vector3< double > & | p, | ||
Vector3< double > & | d, | ||
double & | penetration_depth, | ||
int & | ret | ||
) |
FCL_EXPORT bool fcl::detail::halfspacePlaneIntersect | ( | const Halfspace< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
Plane< S > & | pl, | ||
Vector3< S > & | p, | ||
Vector3< S > & | d, | ||
S & | penetration_depth, | ||
int & | ret | ||
) |
Definition at line 680 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
bool fcl::detail::halfspacePlaneIntersect | ( | const Halfspace< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
Plane< S > & | pl, | ||
Vector3< S > & | p, | ||
Vector3< S > & | d, | ||
S & | penetration_depth, | ||
int & | ret | ||
) |
Definition at line 680 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
template bool fcl::detail::halfspaceTriangleIntersect | ( | const Halfspace< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Vector3< double > & | P1, | ||
const Vector3< double > & | P2, | ||
const Vector3< double > & | P3, | ||
const Transform3< double > & | tf2, | ||
Vector3< double > * | contact_points, | ||
double * | penetration_depth, | ||
Vector3< double > * | normal | ||
) |
FCL_EXPORT bool fcl::detail::halfspaceTriangleIntersect | ( | const Halfspace< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Vector3< S > & | P1, | ||
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3, | ||
const Transform3< S > & | tf2, | ||
Vector3< S > * | contact_points, | ||
S * | penetration_depth, | ||
Vector3< S > * | normal | ||
) |
Definition at line 586 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
bool fcl::detail::halfspaceTriangleIntersect | ( | const Halfspace< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Vector3< S > & | P1, | ||
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3, | ||
const Transform3< S > & | tf2, | ||
Vector3< S > * | contact_points, | ||
S * | penetration_depth, | ||
Vector3< S > * | normal | ||
) |
Definition at line 586 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
bool fcl::detail::initialize | ( | MeshCollisionTraversalNode< BV > & | node, |
BVHModel< BV > & | model1, | ||
Transform3< typename BV::S > & | tf1, | ||
BVHModel< BV > & | model2, | ||
Transform3< typename BV::S > & | tf2, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result, | ||
bool | use_refit, | ||
bool | refit_bottomup | ||
) |
Initialize traversal node for collision between two meshes, given the current transforms.
Definition at line 211 of file mesh_collision_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | MeshCollisionTraversalNode< BV > & | node, |
BVHModel< BV > & | model1, | ||
Transform3< typename BV::S > & | tf1, | ||
BVHModel< BV > & | model2, | ||
Transform3< typename BV::S > & | tf2, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result, | ||
bool | use_refit = false , |
||
bool | refit_bottomup = false |
||
) |
Initialize traversal node for collision between two meshes, given the current transforms.
Definition at line 211 of file mesh_collision_traversal_node-inl.h.
template bool fcl::detail::initialize | ( | MeshCollisionTraversalNodekIOS< double > & | node, |
const BVHModel< kIOS< double >> & | model1, | ||
const Transform3< double > & | tf1, | ||
const BVHModel< kIOS< double >> & | model2, | ||
const Transform3< double > & | tf2, | ||
const CollisionRequest< double > & | request, | ||
CollisionResult< double > & | result | ||
) |
FCL_EXPORT bool fcl::detail::initialize | ( | MeshCollisionTraversalNodekIOS< S > & | node, |
const BVHModel< kIOS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< kIOS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
const CollisionRequest< S > & | request, | ||
CollisionResult< S > & | result | ||
) |
Initialize traversal node for collision between two meshes, specialized for kIOS type.
Definition at line 779 of file mesh_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshCollisionTraversalNodekIOS< S > & | node, |
const BVHModel< kIOS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< kIOS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
const CollisionRequest< S > & | request, | ||
CollisionResult< S > & | result | ||
) |
Initialize traversal node for collision between two meshes, specialized for kIOS type.
Definition at line 779 of file mesh_collision_traversal_node-inl.h.
template bool fcl::detail::initialize | ( | MeshCollisionTraversalNodeOBB< double > & | node, |
const BVHModel< OBB< double >> & | model1, | ||
const Transform3< double > & | tf1, | ||
const BVHModel< OBB< double >> & | model2, | ||
const Transform3< double > & | tf2, | ||
const CollisionRequest< double > & | request, | ||
CollisionResult< double > & | result | ||
) |
FCL_EXPORT bool fcl::detail::initialize | ( | MeshCollisionTraversalNodeOBB< S > & | node, |
const BVHModel< OBB< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< OBB< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
const CollisionRequest< S > & | request, | ||
CollisionResult< S > & | result | ||
) |
Initialize traversal node for collision between two meshes, specialized for OBB type.
Definition at line 749 of file mesh_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshCollisionTraversalNodeOBB< S > & | node, |
const BVHModel< OBB< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< OBB< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
const CollisionRequest< S > & | request, | ||
CollisionResult< S > & | result | ||
) |
Initialize traversal node for collision between two meshes, specialized for OBB type.
Definition at line 749 of file mesh_collision_traversal_node-inl.h.
template bool fcl::detail::initialize | ( | MeshCollisionTraversalNodeOBBRSS< double > & | node, |
const BVHModel< OBBRSS< double >> & | model1, | ||
const Transform3< double > & | tf1, | ||
const BVHModel< OBBRSS< double >> & | model2, | ||
const Transform3< double > & | tf2, | ||
const CollisionRequest< double > & | request, | ||
CollisionResult< double > & | result | ||
) |
FCL_EXPORT bool fcl::detail::initialize | ( | MeshCollisionTraversalNodeOBBRSS< S > & | node, |
const BVHModel< OBBRSS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< OBBRSS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
const CollisionRequest< S > & | request, | ||
CollisionResult< S > & | result | ||
) |
Initialize traversal node for collision between two meshes, specialized for OBBRSS type.
Definition at line 794 of file mesh_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshCollisionTraversalNodeOBBRSS< S > & | node, |
const BVHModel< OBBRSS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< OBBRSS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
const CollisionRequest< S > & | request, | ||
CollisionResult< S > & | result | ||
) |
Initialize traversal node for collision between two meshes, specialized for OBBRSS type.
Definition at line 794 of file mesh_collision_traversal_node-inl.h.
template bool fcl::detail::initialize | ( | MeshCollisionTraversalNodeRSS< double > & | node, |
const BVHModel< RSS< double >> & | model1, | ||
const Transform3< double > & | tf1, | ||
const BVHModel< RSS< double >> & | model2, | ||
const Transform3< double > & | tf2, | ||
const CollisionRequest< double > & | request, | ||
CollisionResult< double > & | result | ||
) |
FCL_EXPORT bool fcl::detail::initialize | ( | MeshCollisionTraversalNodeRSS< S > & | node, |
const BVHModel< RSS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< RSS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
const CollisionRequest< S > & | request, | ||
CollisionResult< S > & | result | ||
) |
Initialize traversal node for collision between two meshes, specialized for RSS type.
Definition at line 764 of file mesh_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshCollisionTraversalNodeRSS< S > & | node, |
const BVHModel< RSS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< RSS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
const CollisionRequest< S > & | request, | ||
CollisionResult< S > & | result | ||
) |
Initialize traversal node for collision between two meshes, specialized for RSS type.
Definition at line 764 of file mesh_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshConservativeAdvancementTraversalNode< BV > & | node, |
BVHModel< BV > & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
BVHModel< BV > & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
typename BV::S | w, | ||
bool | use_refit, | ||
bool | refit_bottomup | ||
) |
Initialize traversal node for conservative advancement computation between two meshes, given the current transforms.
Definition at line 311 of file mesh_conservative_advancement_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | MeshConservativeAdvancementTraversalNode< BV > & | node, |
BVHModel< BV > & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
BVHModel< BV > & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
typename BV::S | w = 1 , |
||
bool | use_refit = false , |
||
bool | refit_bottomup = false |
||
) |
Initialize traversal node for conservative advancement computation between two meshes, given the current transforms.
Definition at line 311 of file mesh_conservative_advancement_traversal_node-inl.h.
template bool fcl::detail::initialize | ( | MeshConservativeAdvancementTraversalNodeOBBRSS< double > & | node, |
const BVHModel< OBBRSS< double >> & | model1, | ||
const Transform3< double > & | tf1, | ||
const BVHModel< OBBRSS< double >> & | model2, | ||
const Transform3< double > & | tf2, | ||
double | w | ||
) |
bool fcl::detail::initialize | ( | MeshConservativeAdvancementTraversalNodeOBBRSS< S > & | node, |
const BVHModel< OBBRSS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< OBBRSS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
S | w | ||
) |
Definition at line 791 of file mesh_conservative_advancement_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | MeshConservativeAdvancementTraversalNodeOBBRSS< S > & | node, |
const BVHModel< OBBRSS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< OBBRSS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
S | w = 1 |
||
) |
Definition at line 791 of file mesh_conservative_advancement_traversal_node-inl.h.
template bool fcl::detail::initialize | ( | MeshConservativeAdvancementTraversalNodeRSS< double > & | node, |
const BVHModel< RSS< double >> & | model1, | ||
const Transform3< double > & | tf1, | ||
const BVHModel< RSS< double >> & | model2, | ||
const Transform3< double > & | tf2, | ||
double | w | ||
) |
bool fcl::detail::initialize | ( | MeshConservativeAdvancementTraversalNodeRSS< S > & | node, |
const BVHModel< RSS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< RSS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
S | w | ||
) |
Initialize traversal node for conservative advancement computation between two meshes, given the current transforms, specialized for RSS.
Definition at line 777 of file mesh_conservative_advancement_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | MeshConservativeAdvancementTraversalNodeRSS< S > & | node, |
const BVHModel< RSS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< RSS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
S | w = 1 |
||
) |
Initialize traversal node for conservative advancement computation between two meshes, given the current transforms, specialized for RSS.
Definition at line 777 of file mesh_conservative_advancement_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | MeshContinuousCollisionTraversalNode< BV > & | node, |
const BVHModel< BV > & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
const BVHModel< BV > & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const CollisionRequest< typename BV::S > & | request | ||
) |
Initialize traversal node for continuous collision detection between two meshes.
Definition at line 183 of file mesh_continuous_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshContinuousCollisionTraversalNode< BV > & | node, |
const BVHModel< BV > & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
const BVHModel< BV > & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const CollisionRequest< typename BV::S > & | request | ||
) |
Initialize traversal node for continuous collision detection between two meshes.
Definition at line 183 of file mesh_continuous_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshDistanceTraversalNode< BV > & | node, |
BVHModel< BV > & | model1, | ||
Transform3< typename BV::S > & | tf1, | ||
BVHModel< BV > & | model2, | ||
Transform3< typename BV::S > & | tf2, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result, | ||
bool | use_refit, | ||
bool | refit_bottomup | ||
) |
Initialize traversal node for distance computation between two meshes, given the current transforms.
Definition at line 157 of file mesh_distance_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | MeshDistanceTraversalNode< BV > & | node, |
BVHModel< BV > & | model1, | ||
Transform3< typename BV::S > & | tf1, | ||
BVHModel< BV > & | model2, | ||
Transform3< typename BV::S > & | tf2, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result, | ||
bool | use_refit = false , |
||
bool | refit_bottomup = false |
||
) |
Initialize traversal node for distance computation between two meshes, given the current transforms.
Definition at line 157 of file mesh_distance_traversal_node-inl.h.
template bool fcl::detail::initialize | ( | MeshDistanceTraversalNodekIOS< double > & | node, |
const BVHModel< kIOS< double >> & | model1, | ||
const Transform3< double > & | tf1, | ||
const BVHModel< kIOS< double >> & | model2, | ||
const Transform3< double > & | tf2, | ||
const DistanceRequest< double > & | request, | ||
DistanceResult< double > & | result | ||
) |
FCL_EXPORT bool fcl::detail::initialize | ( | MeshDistanceTraversalNodekIOS< S > & | node, |
const BVHModel< kIOS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< kIOS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
const DistanceRequest< S > & | request, | ||
DistanceResult< S > & | result | ||
) |
Initialize traversal node for distance computation between two meshes, specialized for kIOS type.
Definition at line 652 of file mesh_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshDistanceTraversalNodekIOS< S > & | node, |
const BVHModel< kIOS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< kIOS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
const DistanceRequest< S > & | request, | ||
DistanceResult< S > & | result | ||
) |
Initialize traversal node for distance computation between two meshes, specialized for kIOS type.
Definition at line 652 of file mesh_distance_traversal_node-inl.h.
template bool fcl::detail::initialize | ( | MeshDistanceTraversalNodeOBBRSS< double > & | node, |
const BVHModel< OBBRSS< double >> & | model1, | ||
const Transform3< double > & | tf1, | ||
const BVHModel< OBBRSS< double >> & | model2, | ||
const Transform3< double > & | tf2, | ||
const DistanceRequest< double > & | request, | ||
DistanceResult< double > & | result | ||
) |
FCL_EXPORT bool fcl::detail::initialize | ( | MeshDistanceTraversalNodeOBBRSS< S > & | node, |
const BVHModel< OBBRSS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< OBBRSS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
const DistanceRequest< S > & | request, | ||
DistanceResult< S > & | result | ||
) |
Initialize traversal node for distance computation between two meshes, specialized for OBBRSS type.
Definition at line 667 of file mesh_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshDistanceTraversalNodeOBBRSS< S > & | node, |
const BVHModel< OBBRSS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< OBBRSS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
const DistanceRequest< S > & | request, | ||
DistanceResult< S > & | result | ||
) |
Initialize traversal node for distance computation between two meshes, specialized for OBBRSS type.
Definition at line 667 of file mesh_distance_traversal_node-inl.h.
template bool fcl::detail::initialize | ( | MeshDistanceTraversalNodeRSS< double > & | node, |
const BVHModel< RSS< double >> & | model1, | ||
const Transform3< double > & | tf1, | ||
const BVHModel< RSS< double >> & | model2, | ||
const Transform3< double > & | tf2, | ||
const DistanceRequest< double > & | request, | ||
DistanceResult< double > & | result | ||
) |
FCL_EXPORT bool fcl::detail::initialize | ( | MeshDistanceTraversalNodeRSS< S > & | node, |
const BVHModel< RSS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< RSS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
const DistanceRequest< S > & | request, | ||
DistanceResult< S > & | result | ||
) |
Initialize traversal node for distance computation between two meshes, specialized for RSS type.
Definition at line 637 of file mesh_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshDistanceTraversalNodeRSS< S > & | node, |
const BVHModel< RSS< S >> & | model1, | ||
const Transform3< S > & | tf1, | ||
const BVHModel< RSS< S >> & | model2, | ||
const Transform3< S > & | tf2, | ||
const DistanceRequest< S > & | request, | ||
DistanceResult< S > & | result | ||
) |
Initialize traversal node for distance computation between two meshes, specialized for RSS type.
Definition at line 637 of file mesh_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshOcTreeCollisionTraversalNode< BV, NarrowPhaseSolver > & | node, |
const BVHModel< BV > & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
const OcTree< typename BV::S > & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const OcTreeSolver< NarrowPhaseSolver > * | otsolver, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result | ||
) |
Initialize traversal node for collision between one mesh and one octree, given current object transform.
Definition at line 79 of file mesh_octree_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshOcTreeDistanceTraversalNode< BV, NarrowPhaseSolver > & | node, |
const BVHModel< BV > & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
const OcTree< typename BV::S > & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const OcTreeSolver< NarrowPhaseSolver > * | otsolver, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result | ||
) |
Initialize traversal node for distance between one mesh and one octree, given current object transform.
Definition at line 79 of file mesh_octree_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshShapeCollisionTraversalNode< BV, Shape, NarrowPhaseSolver > & | node, |
BVHModel< BV > & | model1, | ||
Transform3< typename BV::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result, | ||
bool | use_refit, | ||
bool | refit_bottomup | ||
) |
Initialize traversal node for collision between one mesh and one shape, given current object transform.
Definition at line 137 of file mesh_shape_collision_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | MeshShapeCollisionTraversalNode< BV, Shape, NarrowPhaseSolver > & | node, |
BVHModel< BV > & | model1, | ||
Transform3< typename BV::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result, | ||
bool | use_refit = false , |
||
bool | refit_bottomup = false |
||
) |
Initialize traversal node for collision between one mesh and one shape, given current object transform.
Definition at line 137 of file mesh_shape_collision_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | MeshShapeCollisionTraversalNodekIOS< Shape, NarrowPhaseSolver > & | node, |
const BVHModel< kIOS< typename Shape::S >> & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result | ||
) |
Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type.
Definition at line 442 of file mesh_shape_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshShapeCollisionTraversalNodekIOS< Shape, NarrowPhaseSolver > & | node, |
const BVHModel< kIOS< typename Shape::S >> & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result | ||
) |
Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type.
Definition at line 442 of file mesh_shape_collision_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | MeshShapeCollisionTraversalNodeOBB< Shape, NarrowPhaseSolver > & | node, |
const BVHModel< OBB< typename Shape::S >> & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result | ||
) |
Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type.
Definition at line 410 of file mesh_shape_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshShapeCollisionTraversalNodeOBB< Shape, NarrowPhaseSolver > & | node, |
const BVHModel< OBB< typename Shape::S >> & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result | ||
) |
Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type.
Definition at line 410 of file mesh_shape_collision_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | MeshShapeCollisionTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > & | node, |
const BVHModel< OBBRSS< typename Shape::S >> & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result | ||
) |
Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type.
Definition at line 458 of file mesh_shape_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshShapeCollisionTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > & | node, |
const BVHModel< OBBRSS< typename Shape::S >> & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result | ||
) |
Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type.
Definition at line 458 of file mesh_shape_collision_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | MeshShapeCollisionTraversalNodeRSS< Shape, NarrowPhaseSolver > & | node, |
const BVHModel< RSS< typename Shape::S >> & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result | ||
) |
Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type.
Definition at line 426 of file mesh_shape_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshShapeCollisionTraversalNodeRSS< Shape, NarrowPhaseSolver > & | node, |
const BVHModel< RSS< typename Shape::S >> & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result | ||
) |
Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type.
Definition at line 426 of file mesh_shape_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshShapeConservativeAdvancementTraversalNode< BV, Shape, NarrowPhaseSolver > & | node, |
BVHModel< BV > & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
typename BV::S | w, | ||
bool | use_refit, | ||
bool | refit_bottomup | ||
) |
Definition at line 173 of file mesh_shape_conservative_advancement_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshShapeConservativeAdvancementTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > & | node, |
const BVHModel< OBBRSS< typename Shape::S >> & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
typename Shape::S | w | ||
) |
Definition at line 512 of file mesh_shape_conservative_advancement_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshShapeConservativeAdvancementTraversalNodeRSS< Shape, NarrowPhaseSolver > & | node, |
const BVHModel< RSS< typename Shape::S >> & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
typename Shape::S | w | ||
) |
Definition at line 411 of file mesh_shape_conservative_advancement_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshShapeDistanceTraversalNode< BV, Shape, NarrowPhaseSolver > & | node, |
BVHModel< BV > & | model1, | ||
Transform3< typename BV::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result, | ||
bool | use_refit = false , |
||
bool | refit_bottomup = false |
||
) |
Initialize traversal node for distance computation between one mesh and one shape, given the current transforms.
bool fcl::detail::initialize | ( | MeshShapeDistanceTraversalNode< BV, Shape, NarrowPhaseSolver > & | node, |
BVHModel< BV > & | model1, | ||
Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result, | ||
bool | use_refit, | ||
bool | refit_bottomup | ||
) |
Definition at line 111 of file mesh_shape_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshShapeDistanceTraversalNodekIOS< Shape, NarrowPhaseSolver > & | node, |
const BVHModel< kIOS< typename Shape::S >> & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename Shape::S > & | request, | ||
DistanceResult< typename Shape::S > & | result | ||
) |
Initialize traversal node for distance computation between one mesh and one shape, specialized for kIOS type.
Definition at line 428 of file mesh_shape_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshShapeDistanceTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > & | node, |
const BVHModel< OBBRSS< typename Shape::S >> & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename Shape::S > & | request, | ||
DistanceResult< typename Shape::S > & | result | ||
) |
Initialize traversal node for distance computation between one mesh and one shape, specialized for OBBRSS type.
Definition at line 441 of file mesh_shape_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | MeshShapeDistanceTraversalNodeRSS< Shape, NarrowPhaseSolver > & | node, |
const BVHModel< RSS< typename Shape::S >> & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename Shape::S > & | request, | ||
DistanceResult< typename Shape::S > & | result | ||
) |
Definition at line 415 of file mesh_shape_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | OcTreeCollisionTraversalNode< NarrowPhaseSolver > & | node, |
const OcTree< typename NarrowPhaseSolver::S > & | model1, | ||
const Transform3< typename NarrowPhaseSolver::S > & | tf1, | ||
const OcTree< typename NarrowPhaseSolver::S > & | model2, | ||
const Transform3< typename NarrowPhaseSolver::S > & | tf2, | ||
const OcTreeSolver< NarrowPhaseSolver > * | otsolver, | ||
const CollisionRequest< typename NarrowPhaseSolver::S > & | request, | ||
CollisionResult< typename NarrowPhaseSolver::S > & | result | ||
) |
Initialize traversal node for collision between two octrees, given current object transform.
Definition at line 78 of file octree_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | OcTreeDistanceTraversalNode< NarrowPhaseSolver > & | node, |
const OcTree< typename NarrowPhaseSolver::S > & | model1, | ||
const Transform3< typename NarrowPhaseSolver::S > & | tf1, | ||
const OcTree< typename NarrowPhaseSolver::S > & | model2, | ||
const Transform3< typename NarrowPhaseSolver::S > & | tf2, | ||
const OcTreeSolver< NarrowPhaseSolver > * | otsolver, | ||
const DistanceRequest< typename NarrowPhaseSolver::S > & | request, | ||
DistanceResult< typename NarrowPhaseSolver::S > & | result | ||
) |
Initialize traversal node for distance between two octrees, given current object transform.
Definition at line 80 of file octree_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | OcTreeMeshCollisionTraversalNode< BV, NarrowPhaseSolver > & | node, |
const OcTree< typename BV::S > & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
const BVHModel< BV > & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const OcTreeSolver< NarrowPhaseSolver > * | otsolver, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result | ||
) |
Initialize traversal node for collision between one octree and one mesh, given current object transform.
Definition at line 79 of file octree_mesh_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | OcTreeMeshDistanceTraversalNode< BV, NarrowPhaseSolver > & | node, |
const OcTree< typename BV::S > & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
const BVHModel< BV > & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const OcTreeSolver< NarrowPhaseSolver > * | otsolver, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result | ||
) |
Initialize traversal node for collision between one octree and one mesh, given current object transform.
Definition at line 79 of file octree_mesh_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | OcTreeShapeCollisionTraversalNode< Shape, NarrowPhaseSolver > & | node, |
const OcTree< typename Shape::S > & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const OcTreeSolver< NarrowPhaseSolver > * | otsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result | ||
) |
Initialize traversal node for collision between one octree and one shape, given current object transform.
Definition at line 79 of file octree_shape_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | OcTreeShapeDistanceTraversalNode< Shape, NarrowPhaseSolver > & | node, |
const OcTree< typename Shape::S > & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const OcTreeSolver< NarrowPhaseSolver > * | otsolver, | ||
const DistanceRequest< typename Shape::S > & | request, | ||
DistanceResult< typename Shape::S > & | result | ||
) |
Initialize traversal node for distance between one octree and one shape, given current object transform.
Definition at line 80 of file octree_shape_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | ShapeCollisionTraversalNode< Shape1, Shape2, NarrowPhaseSolver > & | node, |
const Shape1 & | shape1, | ||
const Transform3< typename Shape1::S > & | tf1, | ||
const Shape2 & | shape2, | ||
const Transform3< typename Shape1::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape1::S > & | request, | ||
CollisionResult< typename Shape1::S > & | result | ||
) |
Initialize traversal node for collision between two geometric shapes, given current object transform.
Definition at line 140 of file shape_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | ShapeConservativeAdvancementTraversalNode< Shape1, Shape2, NarrowPhaseSolver > & | node, |
const Shape1 & | shape1, | ||
const Transform3< typename Shape1::S > & | tf1, | ||
const Shape2 & | shape2, | ||
const Transform3< typename Shape1::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver | ||
) |
Definition at line 100 of file shape_conservative_advancement_traversal_node-inl.h.
bool fcl::detail::initialize | ( | ShapeDistanceTraversalNode< Shape1, Shape2, NarrowPhaseSolver > & | node, |
const Shape1 & | shape1, | ||
const Transform3< typename Shape1::S > & | tf1, | ||
const Shape2 & | shape2, | ||
const Transform3< typename Shape1::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename Shape1::S > & | request, | ||
DistanceResult< typename Shape1::S > & | result | ||
) |
Initialize traversal node for distance between two geometric shapes.
Definition at line 106 of file shape_distance_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | ShapeMeshCollisionTraversalNode< Shape, BV, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
BVHModel< BV > & | model2, | ||
Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result, | ||
bool | use_refit, | ||
bool | refit_bottomup | ||
) |
Initialize traversal node for collision between one mesh and one shape, given current object transform.
Definition at line 143 of file shape_mesh_collision_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | ShapeMeshCollisionTraversalNodekIOS< Shape, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const BVHModel< kIOS< typename Shape::S >> & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result | ||
) |
Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type.
Definition at line 379 of file shape_mesh_collision_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | ShapeMeshCollisionTraversalNodeOBB< Shape, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const BVHModel< OBB< typename Shape::S >> & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result | ||
) |
Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type.
Definition at line 345 of file shape_mesh_collision_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | ShapeMeshCollisionTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const BVHModel< OBBRSS< typename Shape::S >> & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result | ||
) |
Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type.
Definition at line 396 of file shape_mesh_collision_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | ShapeMeshCollisionTraversalNodeRSS< Shape, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const BVHModel< RSS< typename Shape::S >> & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result | ||
) |
Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type.
Definition at line 362 of file shape_mesh_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | ShapeMeshConservativeAdvancementTraversalNode< Shape, BV, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
BVHModel< BV > & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
typename BV::S | w, | ||
bool | use_refit, | ||
bool | refit_bottomup | ||
) |
Definition at line 171 of file shape_mesh_conservative_advancement_traversal_node-inl.h.
bool fcl::detail::initialize | ( | ShapeMeshConservativeAdvancementTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const BVHModel< OBBRSS< typename Shape::S >> & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
typename Shape::S | w | ||
) |
Definition at line 390 of file shape_mesh_conservative_advancement_traversal_node-inl.h.
bool fcl::detail::initialize | ( | ShapeMeshConservativeAdvancementTraversalNodeRSS< Shape, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const BVHModel< RSS< typename Shape::S >> & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
typename Shape::S | w | ||
) |
Definition at line 290 of file shape_mesh_conservative_advancement_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | ShapeMeshDistanceTraversalNode< Shape, BV, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
BVHModel< BV > & | model2, | ||
Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result, | ||
bool | use_refit, | ||
bool | refit_bottomup | ||
) |
Initialize traversal node for distance computation between one shape and one mesh, given the current transforms.
Definition at line 112 of file shape_mesh_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | ShapeMeshDistanceTraversalNode< Shape, BV, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
BVHModel< BV > & | model2, | ||
Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result, | ||
bool | use_refit = false , |
||
bool | refit_bottomup = false |
||
) |
Initialize traversal node for distance computation between one shape and one mesh, given the current transforms.
Definition at line 112 of file shape_mesh_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | ShapeMeshDistanceTraversalNodekIOS< Shape, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const BVHModel< kIOS< typename Shape::S >> & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename Shape::S > & | request, | ||
DistanceResult< typename Shape::S > & | result | ||
) |
Initialize traversal node for distance computation between one shape and one mesh, specialized for kIOS type.
Definition at line 366 of file shape_mesh_distance_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | ShapeMeshDistanceTraversalNodekIOS< Shape, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const BVHModel< kIOS< typename Shape::S >> & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename Shape::S > & | request, | ||
DistanceResult< typename Shape::S > & | result | ||
) |
Initialize traversal node for distance computation between one shape and one mesh, specialized for kIOS type.
Definition at line 366 of file shape_mesh_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | ShapeMeshDistanceTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const BVHModel< OBBRSS< typename Shape::S >> & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename Shape::S > & | request, | ||
DistanceResult< typename Shape::S > & | result | ||
) |
Initialize traversal node for distance computation between one shape and one mesh, specialized for OBBRSS type.
Definition at line 379 of file shape_mesh_distance_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | ShapeMeshDistanceTraversalNodeOBBRSS< Shape, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const BVHModel< OBBRSS< typename Shape::S >> & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename Shape::S > & | request, | ||
DistanceResult< typename Shape::S > & | result | ||
) |
Initialize traversal node for distance computation between one shape and one mesh, specialized for OBBRSS type.
Definition at line 379 of file shape_mesh_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | ShapeMeshDistanceTraversalNodeRSS< Shape, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const BVHModel< RSS< typename Shape::S >> & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename Shape::S > & | request, | ||
DistanceResult< typename Shape::S > & | result | ||
) |
Initialize traversal node for distance computation between one shape and one mesh, specialized for RSS type.
Definition at line 353 of file shape_mesh_distance_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::initialize | ( | ShapeMeshDistanceTraversalNodeRSS< Shape, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const BVHModel< RSS< typename Shape::S >> & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename Shape::S > & | request, | ||
DistanceResult< typename Shape::S > & | result | ||
) |
Initialize traversal node for distance computation between one shape and one mesh, specialized for RSS type.
Definition at line 353 of file shape_mesh_distance_traversal_node-inl.h.
bool fcl::detail::initialize | ( | ShapeOcTreeCollisionTraversalNode< Shape, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const OcTree< typename Shape::S > & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const OcTreeSolver< NarrowPhaseSolver > * | otsolver, | ||
const CollisionRequest< typename Shape::S > & | request, | ||
CollisionResult< typename Shape::S > & | result | ||
) |
Initialize traversal node for collision between one shape and one octree, given current object transform.
Definition at line 79 of file shape_octree_collision_traversal_node-inl.h.
bool fcl::detail::initialize | ( | ShapeOcTreeDistanceTraversalNode< Shape, NarrowPhaseSolver > & | node, |
const Shape & | model1, | ||
const Transform3< typename Shape::S > & | tf1, | ||
const OcTree< typename Shape::S > & | model2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const OcTreeSolver< NarrowPhaseSolver > * | otsolver, | ||
const DistanceRequest< typename Shape::S > & | request, | ||
DistanceResult< typename Shape::S > & | result | ||
) |
Initialize traversal node for distance between one shape and one octree, given current object transform.
Definition at line 80 of file shape_octree_distance_traversal_node-inl.h.
template int fcl::detail::intersectRectQuad2 | ( | double | h[2], |
double | p[8], | ||
double | ret[16] | ||
) |
FCL_EXPORT int fcl::detail::intersectRectQuad2 | ( | S | h[2], |
S | p[8], | ||
S | ret[16] | ||
) |
Definition at line 110 of file box_box-inl.h.
int fcl::detail::intersectRectQuad2 | ( | S | h[2], |
S | p[8], | ||
S | ret[16] | ||
) |
Definition at line 110 of file box_box-inl.h.
bool fcl::detail::IsElementInSet | ( | const std::unordered_set< T * > & | S, |
const T * | element | ||
) |
Definition at line 551 of file test_gjk_libccd-inl_epa.cpp.
template void fcl::detail::lineClosestApproach | ( | const Vector3< double > & | pa, |
const Vector3< double > & | ua, | ||
const Vector3< double > & | pb, | ||
const Vector3< double > & | ub, | ||
double * | alpha, | ||
double * | beta | ||
) |
FCL_EXPORT void fcl::detail::lineClosestApproach | ( | const Vector3< S > & | pa, |
const Vector3< S > & | ua, | ||
const Vector3< S > & | pb, | ||
const Vector3< S > & | ub, | ||
S * | alpha, | ||
S * | beta | ||
) |
Definition at line 86 of file box_box-inl.h.
void fcl::detail::lineClosestApproach | ( | const Vector3< S > & | pa, |
const Vector3< S > & | ua, | ||
const Vector3< S > & | pb, | ||
const Vector3< S > & | ub, | ||
S * | alpha, | ||
S * | beta | ||
) |
Definition at line 86 of file box_box-inl.h.
template void fcl::detail::lineSegmentPointClosestToPoint | ( | const Vector3< double > & | p, |
const Vector3< double > & | s1, | ||
const Vector3< double > & | s2, | ||
Vector3< double > & | sp | ||
) |
FCL_EXPORT void fcl::detail::lineSegmentPointClosestToPoint | ( | const Vector3< S > & | p, |
const Vector3< S > & | s1, | ||
const Vector3< S > & | s2, | ||
Vector3< S > & | sp | ||
) |
Definition at line 71 of file sphere_capsule-inl.h.
void fcl::detail::lineSegmentPointClosestToPoint | ( | const Vector3< S > & | p, |
const Vector3< S > & | s1, | ||
const Vector3< S > & | s2, | ||
Vector3< S > & | sp | ||
) |
Definition at line 71 of file sphere_capsule-inl.h.
void fcl::detail::MapFeature1ToFeature2 | ( | const ccd_list_t * | feature1_list, |
const ccd_list_t * | feature2_list, | ||
std::function< bool(const T *, const T *)> | cmp_feature, | ||
std::unordered_set< T * > * | feature1, | ||
std::unordered_set< T * > * | feature2, | ||
std::unordered_map< T *, T * > * | map_feature1_to_feature2 | ||
) |
Definition at line 880 of file test_gjk_libccd-inl_epa.cpp.
FCL_EXPORT S fcl::detail::maximumDistance_mesh | ( | const Vector3< S > *const | ps, |
const Vector3< S > *const | ps2, | ||
Triangle * | ts, | ||
unsigned int * | indices, | ||
int | n, | ||
const Vector3< S > & | query | ||
) |
Definition at line 149 of file geometry-inl.h.
template double fcl::detail::maximumDistance_mesh | ( | const Vector3d *const | ps, |
const Vector3d *const | ps2, | ||
Triangle * | ts, | ||
unsigned int * | indices, | ||
int | n, | ||
const Vector3d & | query | ||
) |
FCL_EXPORT S fcl::detail::maximumDistance_pointcloud | ( | const Vector3< S > *const | ps, |
const Vector3< S > *const | ps2, | ||
unsigned int * | indices, | ||
int | n, | ||
const Vector3< S > & | query | ||
) |
Definition at line 194 of file geometry-inl.h.
template double fcl::detail::maximumDistance_pointcloud | ( | const Vector3d *const | ps, |
const Vector3d *const | ps2, | ||
unsigned int * | indices, | ||
int | n, | ||
const Vector3d & | query | ||
) |
FCL_EXPORT void fcl::detail::meshCollisionOrientedNodeLeafTesting | ( | int | b1, |
int | b2, | ||
const BVHModel< BV > * | model1, | ||
const BVHModel< BV > * | model2, | ||
Vector3< typename BV::S > * | vertices1, | ||
Vector3< typename BV::S > * | vertices2, | ||
Triangle * | tri_indices1, | ||
Triangle * | tri_indices2, | ||
const Matrix3< typename BV::S > & | R, | ||
const Vector3< typename BV::S > & | T, | ||
const Transform3< typename BV::S > & | tf1, | ||
const Transform3< typename BV::S > & | tf2, | ||
bool | enable_statistics, | ||
typename BV::S | cost_density, | ||
int & | num_leaf_tests, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result | ||
) |
Definition at line 527 of file mesh_collision_traversal_node-inl.h.
void fcl::detail::meshCollisionOrientedNodeLeafTesting | ( | int | b1, |
int | b2, | ||
const BVHModel< BV > * | model1, | ||
const BVHModel< BV > * | model2, | ||
Vector3< typename BV::S > * | vertices1, | ||
Vector3< typename BV::S > * | vertices2, | ||
Triangle * | tri_indices1, | ||
Triangle * | tri_indices2, | ||
const Matrix3< typename BV::S > & | R, | ||
const Vector3< typename BV::S > & | T, | ||
const Transform3< typename BV::S > & | tf1, | ||
const Transform3< typename BV::S > & | tf2, | ||
bool | enable_statistics, | ||
typename BV::S | cost_density, | ||
int & | num_leaf_tests, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result | ||
) |
Definition at line 527 of file mesh_collision_traversal_node-inl.h.
FCL_EXPORT void fcl::detail::meshCollisionOrientedNodeLeafTesting | ( | int | b1, |
int | b2, | ||
const BVHModel< BV > * | model1, | ||
const BVHModel< BV > * | model2, | ||
Vector3< typename BV::S > * | vertices1, | ||
Vector3< typename BV::S > * | vertices2, | ||
Triangle * | tri_indices1, | ||
Triangle * | tri_indices2, | ||
const Transform3< typename BV::S > & | tf, | ||
const Transform3< typename BV::S > & | tf1, | ||
const Transform3< typename BV::S > & | tf2, | ||
bool | enable_statistics, | ||
typename BV::S | cost_density, | ||
int & | num_leaf_tests, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result | ||
) |
Definition at line 624 of file mesh_collision_traversal_node-inl.h.
void fcl::detail::meshCollisionOrientedNodeLeafTesting | ( | int | b1, |
int | b2, | ||
const BVHModel< BV > * | model1, | ||
const BVHModel< BV > * | model2, | ||
Vector3< typename BV::S > * | vertices1, | ||
Vector3< typename BV::S > * | vertices2, | ||
Triangle * | tri_indices1, | ||
Triangle * | tri_indices2, | ||
const Transform3< typename BV::S > & | tf, | ||
const Transform3< typename BV::S > & | tf1, | ||
const Transform3< typename BV::S > & | tf2, | ||
bool | enable_statistics, | ||
typename BV::S | cost_density, | ||
int & | num_leaf_tests, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result | ||
) |
Definition at line 624 of file mesh_collision_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::meshConservativeAdvancementOrientedNodeCanStop | ( | typename BV::S | c, |
typename BV::S | min_distance, | ||
typename BV::S | abs_err, | ||
typename BV::S | rel_err, | ||
typename BV::S | w, | ||
const BVHModel< BV > * | model1, | ||
const BVHModel< BV > * | model2, | ||
const MotionBase< typename BV::S > * | motion1, | ||
const MotionBase< typename BV::S > * | motion2, | ||
std::vector< ConservativeAdvancementStackData< typename BV::S >> & | stack, | ||
typename BV::S & | delta_t | ||
) |
Definition at line 581 of file mesh_conservative_advancement_traversal_node-inl.h.
bool fcl::detail::meshConservativeAdvancementOrientedNodeCanStop | ( | typename BV::S | c, |
typename BV::S | min_distance, | ||
typename BV::S | abs_err, | ||
typename BV::S | rel_err, | ||
typename BV::S | w, | ||
const BVHModel< BV > * | model1, | ||
const BVHModel< BV > * | model2, | ||
const MotionBase< typename BV::S > * | motion1, | ||
const MotionBase< typename BV::S > * | motion2, | ||
std::vector< ConservativeAdvancementStackData< typename BV::S >> & | stack, | ||
typename BV::S & | delta_t | ||
) |
Definition at line 581 of file mesh_conservative_advancement_traversal_node-inl.h.
FCL_EXPORT void fcl::detail::meshConservativeAdvancementOrientedNodeLeafTesting | ( | int | b1, |
int | b2, | ||
const BVHModel< BV > * | model1, | ||
const BVHModel< BV > * | model2, | ||
const Triangle * | tri_indices1, | ||
const Triangle * | tri_indices2, | ||
const Vector3< typename BV::S > * | vertices1, | ||
const Vector3< typename BV::S > * | vertices2, | ||
const Matrix3< typename BV::S > & | R, | ||
const Vector3< typename BV::S > & | T, | ||
const MotionBase< typename BV::S > * | motion1, | ||
const MotionBase< typename BV::S > * | motion2, | ||
bool | enable_statistics, | ||
typename BV::S & | min_distance, | ||
Vector3< typename BV::S > & | p1, | ||
Vector3< typename BV::S > & | p2, | ||
int & | last_tri_id1, | ||
int & | last_tri_id2, | ||
typename BV::S & | delta_t, | ||
int & | num_leaf_tests | ||
) |
n is the local frame of object 1, pointing from object 1 to object2
turn n into the global frame, pointing from object 1 to object 2
Definition at line 664 of file mesh_conservative_advancement_traversal_node-inl.h.
void fcl::detail::meshConservativeAdvancementOrientedNodeLeafTesting | ( | int | b1, |
int | b2, | ||
const BVHModel< BV > * | model1, | ||
const BVHModel< BV > * | model2, | ||
const Triangle * | tri_indices1, | ||
const Triangle * | tri_indices2, | ||
const Vector3< typename BV::S > * | vertices1, | ||
const Vector3< typename BV::S > * | vertices2, | ||
const Matrix3< typename BV::S > & | R, | ||
const Vector3< typename BV::S > & | T, | ||
const MotionBase< typename BV::S > * | motion1, | ||
const MotionBase< typename BV::S > * | motion2, | ||
bool | enable_statistics, | ||
typename BV::S & | min_distance, | ||
Vector3< typename BV::S > & | p1, | ||
Vector3< typename BV::S > & | p2, | ||
int & | last_tri_id1, | ||
int & | last_tri_id2, | ||
typename BV::S & | delta_t, | ||
int & | num_leaf_tests | ||
) |
n is the local frame of object 1, pointing from object 1 to object2
turn n into the global frame, pointing from object 1 to object 2
Definition at line 664 of file mesh_conservative_advancement_traversal_node-inl.h.
FCL_EXPORT bool fcl::detail::meshConservativeAdvancementTraversalNodeCanStop | ( | typename BV::S | c, |
typename BV::S | min_distance, | ||
typename BV::S | abs_err, | ||
typename BV::S | rel_err, | ||
typename BV::S | w, | ||
const BVHModel< BV > * | model1, | ||
const BVHModel< BV > * | model2, | ||
const MotionBase< typename BV::S > * | motion1, | ||
const MotionBase< typename BV::S > * | motion2, | ||
std::vector< ConservativeAdvancementStackData< typename BV::S >> & | stack, | ||
typename BV::S & | delta_t | ||
) |
Definition at line 503 of file mesh_conservative_advancement_traversal_node-inl.h.
bool fcl::detail::meshConservativeAdvancementTraversalNodeCanStop | ( | typename BV::S | c, |
typename BV::S | min_distance, | ||
typename BV::S | abs_err, | ||
typename BV::S | rel_err, | ||
typename BV::S | w, | ||
const BVHModel< BV > * | model1, | ||
const BVHModel< BV > * | model2, | ||
const MotionBase< typename BV::S > * | motion1, | ||
const MotionBase< typename BV::S > * | motion2, | ||
std::vector< ConservativeAdvancementStackData< typename BV::S >> & | stack, | ||
typename BV::S & | delta_t | ||
) |
Definition at line 503 of file mesh_conservative_advancement_traversal_node-inl.h.
FCL_DEPRECATED_EXPORT void fcl::detail::meshDistanceOrientedNodeLeafTesting | ( | int | b1, |
int | b2, | ||
const BVHModel< BV > * | model1, | ||
const BVHModel< BV > * | model2, | ||
Vector3< typename BV::S > * | vertices1, | ||
Vector3< typename BV::S > * | vertices2, | ||
Triangle * | tri_indices1, | ||
Triangle * | tri_indices2, | ||
const Matrix3< typename BV::S > & | R, | ||
const Vector3< typename BV::S > & | T, | ||
bool | enable_statistics, | ||
int & | num_leaf_tests, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result | ||
) |
Definition at line 402 of file mesh_distance_traversal_node-inl.h.
void fcl::detail::meshDistanceOrientedNodeLeafTesting | ( | int | b1, |
int | b2, | ||
const BVHModel< BV > * | model1, | ||
const BVHModel< BV > * | model2, | ||
Vector3< typename BV::S > * | vertices1, | ||
Vector3< typename BV::S > * | vertices2, | ||
Triangle * | tri_indices1, | ||
Triangle * | tri_indices2, | ||
const Matrix3< typename BV::S > & | R, | ||
const Vector3< typename BV::S > & | T, | ||
bool | enable_statistics, | ||
int & | num_leaf_tests, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result | ||
) |
Definition at line 402 of file mesh_distance_traversal_node-inl.h.
FCL_EXPORT void fcl::detail::meshDistanceOrientedNodeLeafTesting | ( | int | b1, |
int | b2, | ||
const BVHModel< BV > * | model1, | ||
const BVHModel< BV > * | model2, | ||
Vector3< typename BV::S > * | vertices1, | ||
Vector3< typename BV::S > * | vertices2, | ||
Triangle * | tri_indices1, | ||
Triangle * | tri_indices2, | ||
const Transform3< typename BV::S > & | tf, | ||
bool | enable_statistics, | ||
int & | num_leaf_tests, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result | ||
) |
Definition at line 453 of file mesh_distance_traversal_node-inl.h.
void fcl::detail::meshDistanceOrientedNodeLeafTesting | ( | int | b1, |
int | b2, | ||
const BVHModel< BV > * | model1, | ||
const BVHModel< BV > * | model2, | ||
Vector3< typename BV::S > * | vertices1, | ||
Vector3< typename BV::S > * | vertices2, | ||
Triangle * | tri_indices1, | ||
Triangle * | tri_indices2, | ||
const Transform3< typename BV::S > & | tf, | ||
bool | enable_statistics, | ||
int & | num_leaf_tests, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result | ||
) |
Definition at line 453 of file mesh_distance_traversal_node-inl.h.
FCL_EXPORT void fcl::detail::meshShapeCollisionOrientedNodeLeafTesting | ( | int | b1, |
int | b2, | ||
const BVHModel< BV > * | model1, | ||
const Shape & | model2, | ||
Vector3< typename BV::S > * | vertices, | ||
Triangle * | tri_indices, | ||
const Transform3< typename BV::S > & | tf1, | ||
const Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
bool | enable_statistics, | ||
typename BV::S | cost_density, | ||
int & | num_leaf_tests, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result | ||
) |
Definition at line 192 of file mesh_shape_collision_traversal_node-inl.h.
void fcl::detail::meshShapeCollisionOrientedNodeLeafTesting | ( | int | b1, |
int | b2, | ||
const BVHModel< BV > * | model1, | ||
const Shape & | model2, | ||
Vector3< typename BV::S > * | vertices, | ||
Triangle * | tri_indices, | ||
const Transform3< typename BV::S > & | tf1, | ||
const Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
bool | enable_statistics, | ||
typename BV::S | cost_density, | ||
int & | num_leaf_tests, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result | ||
) |
Definition at line 192 of file mesh_shape_collision_traversal_node-inl.h.
bool fcl::detail::meshShapeConservativeAdvancementOrientedNodeCanStop | ( | typename BV::S | c, |
typename BV::S | min_distance, | ||
typename BV::S | abs_err, | ||
typename BV::S | rel_err, | ||
typename BV::S | w, | ||
const BVHModel< BV > * | model1, | ||
const Shape & | model2, | ||
const BV & | model2_bv, | ||
const MotionBase< typename BV::S > * | motion1, | ||
const MotionBase< typename BV::S > * | motion2, | ||
std::vector< ConservativeAdvancementStackData< typename BV::S >> & | stack, | ||
typename BV::S & | delta_t | ||
) |
Definition at line 285 of file mesh_shape_conservative_advancement_traversal_node-inl.h.
void fcl::detail::meshShapeConservativeAdvancementOrientedNodeLeafTesting | ( | int | b1, |
int | , | ||
const BVHModel< BV > * | model1, | ||
const Shape & | model2, | ||
const BV & | model2_bv, | ||
Vector3< typename BV::S > * | vertices, | ||
Triangle * | tri_indices, | ||
const Transform3< typename BV::S > & | tf1, | ||
const Transform3< typename BV::S > & | tf2, | ||
const MotionBase< typename BV::S > * | motion1, | ||
const MotionBase< typename BV::S > * | motion2, | ||
const NarrowPhaseSolver * | nsolver, | ||
bool | enable_statistics, | ||
typename BV::S & | min_distance, | ||
Vector3< typename BV::S > & | p1, | ||
Vector3< typename BV::S > & | p2, | ||
int & | last_tri_id, | ||
typename BV::S & | delta_t, | ||
int & | num_leaf_tests | ||
) |
Definition at line 217 of file mesh_shape_conservative_advancement_traversal_node-inl.h.
void fcl::detail::meshShapeDistanceOrientedNodeLeafTesting | ( | int | b1, |
int | , | ||
const BVHModel< BV > * | model1, | ||
const Shape & | model2, | ||
Vector3< typename BV::S > * | vertices, | ||
Triangle * | tri_indices, | ||
const Transform3< typename BV::S > & | tf1, | ||
const Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
bool | enable_statistics, | ||
int & | num_leaf_tests, | ||
const DistanceRequest< typename BV::S > & | , | ||
DistanceResult< typename BV::S > & | result | ||
) |
Definition at line 164 of file mesh_shape_distance_traversal_node-inl.h.
bool fcl::detail::nearestPointInBox | ( | const Vector3< S > & | size, |
const Vector3< S > & | p_BQ, | ||
Vector3< S > * | p_BN_ptr | ||
) |
Definition at line 73 of file sphere_box-inl.h.
bool fcl::detail::nearestPointInCylinder | ( | const S & | height, |
const S & | radius, | ||
const Vector3< S > & | p_CQ, | ||
Vector3< S > * | p_CN_ptr | ||
) |
Definition at line 79 of file sphere_cylinder-inl.h.
bool fcl::detail::nodeBaseLess | ( | NodeBase< BV > * | a, |
NodeBase< BV > * | b, | ||
int | d | ||
) |
Compare two nodes accoording to the d-th dimension of node center.
Definition at line 1050 of file hierarchy_tree-inl.h.
std::size_t fcl::detail::orientedBVHShapeCollide | ( | const CollisionGeometry< typename BV::S > * | o1, |
const Transform3< typename BV::S > & | tf1, | ||
const CollisionGeometry< typename BV::S > * | o2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result | ||
) |
Definition at line 378 of file collision_func_matrix-inl.h.
Shape::S fcl::detail::orientedBVHShapeDistance | ( | const CollisionGeometry< typename Shape::S > * | o1, |
const Transform3< typename Shape::S > & | tf1, | ||
const CollisionGeometry< typename Shape::S > * | o2, | ||
const Transform3< typename Shape::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename Shape::S > & | request, | ||
DistanceResult< typename Shape::S > & | result | ||
) |
Definition at line 259 of file distance_func_matrix-inl.h.
std::size_t fcl::detail::orientedMeshCollide | ( | const CollisionGeometry< typename BV::S > * | o1, |
const Transform3< typename BV::S > & | tf1, | ||
const CollisionGeometry< typename BV::S > * | o2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result | ||
) |
Definition at line 572 of file collision_func_matrix-inl.h.
BV::S fcl::detail::orientedMeshDistance | ( | const CollisionGeometry< typename BV::S > * | o1, |
const Transform3< typename BV::S > & | tf1, | ||
const CollisionGeometry< typename BV::S > * | o2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const DistanceRequest< typename BV::S > & | request, | ||
DistanceResult< typename BV::S > & | result | ||
) |
Definition at line 387 of file distance_func_matrix-inl.h.
bool fcl::detail::overlap | ( | S | a1, |
S | a2, | ||
S | b1, | ||
S | b2 | ||
) |
returns 1 if the intervals overlap, and 0 otherwise
Definition at line 498 of file interval_tree-inl.h.
template bool fcl::detail::planeHalfspaceIntersect | ( | const Plane< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Halfspace< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
Plane< double > & | pl, | ||
Vector3< double > & | p, | ||
Vector3< double > & | d, | ||
double & | penetration_depth, | ||
int & | ret | ||
) |
FCL_EXPORT bool fcl::detail::planeHalfspaceIntersect | ( | const Plane< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
Plane< S > & | pl, | ||
Vector3< S > & | p, | ||
Vector3< S > & | d, | ||
S & | penetration_depth, | ||
int & | ret | ||
) |
return whether plane collides with halfspace if the separation plane of the halfspace is parallel with the plane return code 1, if the plane's normal is the same with halfspace's normal and plane is inside halfspace, also return plane in pl return code 2, if the plane's normal is oppositie to the halfspace's normal and plane is inside halfspace, also return plane in pl plane is outside halfspace, collision-free if not parallel return the intersection ray, return code 3. ray origin is p and direction is d
Definition at line 624 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
bool fcl::detail::planeHalfspaceIntersect | ( | const Plane< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
Plane< S > & | pl, | ||
Vector3< S > & | p, | ||
Vector3< S > & | d, | ||
S & | penetration_depth, | ||
int & | ret | ||
) |
return whether plane collides with halfspace if the separation plane of the halfspace is parallel with the plane return code 1, if the plane's normal is the same with halfspace's normal and plane is inside halfspace, also return plane in pl return code 2, if the plane's normal is oppositie to the halfspace's normal and plane is inside halfspace, also return plane in pl plane is outside halfspace, collision-free if not parallel return the intersection ray, return code 3. ray origin is p and direction is d
Definition at line 624 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
template bool fcl::detail::planeIntersect | ( | const Plane< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Plane< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
bool fcl::detail::planeIntersect | ( | const Plane< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | |||
) |
Definition at line 762 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
FCL_EXPORT bool fcl::detail::planeIntersect | ( | const Plane< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 762 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
float fcl::detail::planeIntersectTolerance | ( | ) |
Definition at line 48 of file narrowphase/detail/primitive_shape_algorithm/plane.cpp.
FCL_EXPORT S fcl::detail::planeIntersectTolerance | ( | ) |
Definition at line 115 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
S fcl::detail::planeIntersectTolerance | ( | ) |
Definition at line 115 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
template bool fcl::detail::planeTriangleIntersect | ( | const Plane< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Vector3< double > & | P1, | ||
const Vector3< double > & | P2, | ||
const Vector3< double > & | P3, | ||
const Transform3< double > & | tf2, | ||
Vector3< double > * | contact_points, | ||
double * | penetration_depth, | ||
Vector3< double > * | normal | ||
) |
FCL_EXPORT bool fcl::detail::planeTriangleIntersect | ( | const Plane< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Vector3< S > & | P1, | ||
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3, | ||
const Transform3< S > & | tf2, | ||
Vector3< S > * | contact_points, | ||
S * | penetration_depth, | ||
Vector3< S > * | normal | ||
) |
Definition at line 682 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
bool fcl::detail::planeTriangleIntersect | ( | const Plane< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Vector3< S > & | P1, | ||
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3, | ||
const Transform3< S > & | tf2, | ||
Vector3< S > * | contact_points, | ||
S * | penetration_depth, | ||
Vector3< S > * | normal | ||
) |
Definition at line 682 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
template bool fcl::detail::projectInTriangle | ( | const Vector3< double > & | p1, |
const Vector3< double > & | p2, | ||
const Vector3< double > & | p3, | ||
const Vector3< double > & | normal, | ||
const Vector3< double > & | p | ||
) |
FCL_EXPORT bool fcl::detail::projectInTriangle | ( | const Vector3< S > & | p1, |
const Vector3< S > & | p2, | ||
const Vector3< S > & | p3, | ||
const Vector3< S > & | normal, | ||
const Vector3< S > & | p | ||
) |
Whether a point's projection is in a triangle.
Definition at line 113 of file sphere_triangle-inl.h.
bool fcl::detail::projectInTriangle | ( | const Vector3< S > & | p1, |
const Vector3< S > & | p2, | ||
const Vector3< S > & | p3, | ||
const Vector3< S > & | normal, | ||
const Vector3< S > & | p | ||
) |
Whether a point's projection is in a triangle.
Definition at line 113 of file sphere_triangle-inl.h.
template void fcl::detail::propagateBVHFrontListCollisionRecurse | ( | CollisionTraversalNodeBase< double > * | node, |
BVHFrontList * | front_list | ||
) |
FCL_EXPORT void fcl::detail::propagateBVHFrontListCollisionRecurse | ( | CollisionTraversalNodeBase< S > * | node, |
BVHFrontList * | front_list | ||
) |
Recurse function for front list propagation.
Definition at line 465 of file traversal_recurse-inl.h.
template double fcl::detail::segmentSqrDistance | ( | const Vector3< double > & | from, |
const Vector3< double > & | to, | ||
const Vector3< double > & | p, | ||
Vector3< double > & | nearest | ||
) |
FCL_EXPORT S fcl::detail::segmentSqrDistance | ( | const Vector3< S > & | from, |
const Vector3< S > & | to, | ||
const Vector3< S > & | p, | ||
Vector3< S > & | nearest | ||
) |
the minimum distance from a point to a line
Definition at line 84 of file sphere_triangle-inl.h.
S fcl::detail::segmentSqrDistance | ( | const Vector3< S > & | from, |
const Vector3< S > & | to, | ||
const Vector3< S > & | p, | ||
Vector3< S > & | nearest | ||
) |
the minimum distance from a point to a line
Definition at line 84 of file sphere_triangle-inl.h.
size_t fcl::detail::select | ( | const BV & | query, |
const NodeBase< BV > & | node1, | ||
const NodeBase< BV > & | node2 | ||
) |
select from node1 and node2 which is close to a given query bounding volume. 0 for node1 and 1 for node2
Definition at line 1089 of file hierarchy_tree-inl.h.
size_t fcl::detail::select | ( | const NodeBase< BV > & | query, |
const NodeBase< BV > & | node1, | ||
const NodeBase< BV > & | node2 | ||
) |
select from node1 and node2 which is close to a given query. 0 for node1 and 1 for node2
Definition at line 1079 of file hierarchy_tree-inl.h.
template void fcl::detail::selfCollide | ( | CollisionTraversalNodeBase< double > * | node, |
BVHFrontList * | front_list | ||
) |
void fcl::detail::selfCollide | ( | CollisionTraversalNodeBase< S > * | node, |
BVHFrontList * | front_list | ||
) |
self collision on collision traversal node; can use front list to accelerate
Definition at line 122 of file collision_node-inl.h.
FCL_EXPORT void fcl::detail::selfCollide | ( | CollisionTraversalNodeBase< S > * | node, |
BVHFrontList * | front_list = nullptr |
||
) |
self collision on collision traversal node; can use front list to accelerate
Definition at line 122 of file collision_node-inl.h.
template void fcl::detail::selfCollisionRecurse | ( | CollisionTraversalNodeBase< double > * | node, |
int | b, | ||
BVHFrontList * | front_list | ||
) |
FCL_EXPORT void fcl::detail::selfCollisionRecurse | ( | CollisionTraversalNodeBase< S > * | node, |
int | b, | ||
BVHFrontList * | front_list | ||
) |
Recurse function for self collision. Make sure node is set correctly so that the first and second tree are the same.
Recurse function for self collision Make sure node is set correctly so that the first and second tree are the same
Definition at line 238 of file traversal_recurse-inl.h.
void fcl::detail::SetUpBoxToBox | ( | const Transform3< S > & | X_WF, |
void ** | o1, | ||
void ** | o2, | ||
ccd_t * | ccd, | ||
fcl::Transform3< S > * | X_FB1, | ||
fcl::Transform3< S > * | X_FB2 | ||
) |
Definition at line 1336 of file test_gjk_libccd-inl_epa.cpp.
bool fcl::detail::setupMeshCollisionOrientedNode | ( | OrientedNode & | node, |
const BVHModel< BV > & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
const BVHModel< BV > & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result | ||
) |
Definition at line 716 of file mesh_collision_traversal_node-inl.h.
bool fcl::detail::setupMeshConservativeAdvancementOrientedDistanceNode | ( | OrientedDistanceNode & | node, |
const BVHModel< BV > & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
const BVHModel< BV > & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
typename BV::S | w | ||
) |
Definition at line 750 of file mesh_conservative_advancement_traversal_node-inl.h.
|
static |
Definition at line 606 of file mesh_distance_traversal_node-inl.h.
bool fcl::detail::setupMeshShapeCollisionOrientedNode | ( | OrientedNode< Shape, NarrowPhaseSolver > & | node, |
const BVHModel< BV > & | model1, | ||
const Transform3< typename BV::S > & | tf1, | ||
const Shape & | model2, | ||
const Transform3< typename BV::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename BV::S > & | request, | ||
CollisionResult< typename BV::S > & | result | ||
) |
Definition at line 377 of file mesh_shape_collision_traversal_node-inl.h.
|
static |
Definition at line 386 of file mesh_shape_distance_traversal_node-inl.h.
|
static |
Definition at line 313 of file shape_mesh_collision_traversal_node-inl.h.
|
static |
Definition at line 321 of file shape_mesh_distance_traversal_node-inl.h.
BV::S fcl::detail::ShapeBVHConservativeAdvancement | ( | const CollisionGeometry< typename BV::S > * | o1, |
const MotionBase< typename BV::S > * | motion1, | ||
const CollisionGeometry< typename BV::S > * | o2, | ||
const MotionBase< typename BV::S > * | motion2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const ContinuousCollisionRequest< typename BV::S > & | request, | ||
ContinuousCollisionResult< typename BV::S > & | result | ||
) |
Definition at line 736 of file conservative_advancement_func_matrix-inl.h.
Shape1::S fcl::detail::ShapeConservativeAdvancement | ( | const CollisionGeometry< typename Shape1::S > * | o1, |
const MotionBase< typename Shape1::S > * | motion1, | ||
const CollisionGeometry< typename Shape1::S > * | o2, | ||
const MotionBase< typename Shape1::S > * | motion2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const ContinuousCollisionRequest< typename Shape1::S > & | request, | ||
ContinuousCollisionResult< typename Shape1::S > & | result | ||
) |
Definition at line 715 of file conservative_advancement_func_matrix-inl.h.
std::size_t fcl::detail::ShapeShapeCollide | ( | const CollisionGeometry< typename Shape1::S > * | o1, |
const Transform3< typename Shape1::S > & | tf1, | ||
const CollisionGeometry< typename Shape1::S > * | o2, | ||
const Transform3< typename Shape1::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const CollisionRequest< typename Shape1::S > & | request, | ||
CollisionResult< typename Shape1::S > & | result | ||
) |
Definition at line 279 of file collision_func_matrix-inl.h.
Shape1::S fcl::detail::ShapeShapeDistance | ( | const CollisionGeometry< typename Shape1::S > * | o1, |
const Transform3< typename Shape1::S > & | tf1, | ||
const CollisionGeometry< typename Shape1::S > * | o2, | ||
const Transform3< typename Shape1::S > & | tf2, | ||
const NarrowPhaseSolver * | nsolver, | ||
const DistanceRequest< typename Shape1::S > & | request, | ||
DistanceResult< typename Shape1::S > & | result | ||
) |
Definition at line 208 of file distance_func_matrix-inl.h.
|
static |
Basic shape to ccd shape
Definition at line 2265 of file gjk_libccd-inl.h.
template bool fcl::detail::sphereBoxDistance | ( | const Sphere< double > & | sphere, |
const Transform3< double > & | X_FS, | ||
const Box< double > & | box, | ||
const Transform3< double > & | X_FB, | ||
double * | distance, | ||
Vector3< double > * | p_FSb, | ||
Vector3< double > * | p_FBs | ||
) |
FCL_EXPORT bool fcl::detail::sphereBoxDistance | ( | const Sphere< S > & | sphere, |
const Transform3< S > & | X_FS, | ||
const Box< S > & | box, | ||
const Transform3< S > & | X_FB, | ||
S * | distance, | ||
Vector3< S > * | p_FSb, | ||
Vector3< S > * | p_FBs | ||
) |
Evaluate the minimum separating distance between a sphere and box. If separated, the nearest points on each shape will be returned in frame F.
sphere | The sphere geometry. |
X_FS | The pose of the sphere S in the common frame F. |
box | The box geometry. |
X_FB | The pose of the box B in the common frame F. |
distance[out] | (optional) The separating distance between the box and sphere. Set to -1 if the shapes are penetrating. |
p_FSb[out] | (optional) The closest point on the sphere to the box measured and expressed in frame F. |
p_FBs[out] | (optional) The closest point on the box to the sphere measured and expressed in frame F. |
S | The scalar parameter (must be a valid Eigen scalar). |
Definition at line 183 of file sphere_box-inl.h.
template bool fcl::detail::sphereBoxIntersect | ( | const Sphere< double > & | sphere, |
const Transform3< double > & | X_FS, | ||
const Box< double > & | box, | ||
const Transform3< double > & | X_FB, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
FCL_EXPORT bool fcl::detail::sphereBoxIntersect | ( | const Sphere< S > & | sphere, |
const Transform3< S > & | X_FS, | ||
const Box< S > & | box, | ||
const Transform3< S > & | X_FB, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Detect collision between the sphere and box. If colliding, return characterization of collision in the provided vector.
The reported depth is guaranteed to be continuous with respect to the relative pose of the two objects. The normal and contact position are guaranteed to be continuous while the sphere center lies outside the box. However, if the sphere center lies inside the box, there are regions of discontinuity in both normal and contact position. This is due to the fact that there is not necessarily a unique characterization of penetration depth (i.e., the sphere center may be equidistant to multiple faces). In this case, the faces are arbitrarily prioritized and the face with the highest priority is used to compute normal and position. The priority order is +x, -x, +y, -y, +z, and -z. For example:
sphere | The sphere geometry. |
X_FS | The pose of the sphere S in the common frame F. |
box | The box geometry. |
X_FB | The pose of the box B in the common frame F. |
contacts[out] | (optional) If the shapes collide, the contact point data will be appended to the end of this vector. |
S | The scalar parameter (must be a valid Eigen scalar). |
Definition at line 98 of file sphere_box-inl.h.
template bool fcl::detail::sphereCapsuleDistance | ( | const Sphere< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Capsule< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
double * | dist, | ||
Vector3< double > * | p1, | ||
Vector3< double > * | p2 | ||
) |
FCL_EXPORT bool fcl::detail::sphereCapsuleDistance | ( | const Sphere< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Capsule< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
S * | dist, | ||
Vector3< S > * | p1, | ||
Vector3< S > * | p2 | ||
) |
Definition at line 125 of file sphere_capsule-inl.h.
bool fcl::detail::sphereCapsuleDistance | ( | const Sphere< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Capsule< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
S * | dist, | ||
Vector3< S > * | p1, | ||
Vector3< S > * | p2 | ||
) |
Definition at line 125 of file sphere_capsule-inl.h.
template bool fcl::detail::sphereCapsuleIntersect | ( | const Sphere< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Capsule< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
FCL_EXPORT bool fcl::detail::sphereCapsuleIntersect | ( | const Sphere< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Capsule< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 91 of file sphere_capsule-inl.h.
bool fcl::detail::sphereCapsuleIntersect | ( | const Sphere< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Capsule< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 91 of file sphere_capsule-inl.h.
template bool fcl::detail::sphereCylinderDistance | ( | const Sphere< double > & | sphere, |
const Transform3< double > & | X_FS, | ||
const Cylinder< double > & | cylinder, | ||
const Transform3< double > & | X_FC, | ||
double * | distance, | ||
Vector3< double > * | p_FSc, | ||
Vector3< double > * | p_FCs | ||
) |
FCL_EXPORT bool fcl::detail::sphereCylinderDistance | ( | const Sphere< S > & | sphere, |
const Transform3< S > & | X_FS, | ||
const Cylinder< S > & | cylinder, | ||
const Transform3< S > & | X_FC, | ||
S * | distance, | ||
Vector3< S > * | p_FSc, | ||
Vector3< S > * | p_FCs | ||
) |
Evaluate the minimum separating distance between a sphere and cylinder. If separated, the nearest points on each shape will be returned in frame F.
sphere | The sphere geometry. |
X_FS | The pose of the sphere S in the common frame F. |
cylinder | The cylinder geometry. |
X_FC | The pose of the cylinder C in the common frame F. |
distance[out] | (optional) The separating distance between the cylinder and sphere. Set to -1 if the shapes are penetrating. |
p_FSc[out] | (optional) The closest point on the sphere to the cylinder measured and expressed in frame F. |
p_FCs[out] | (optional) The closest point on the cylinder to the sphere measured and expressed in frame F. |
S | The scalar parameter (must be a valid Eigen scalar). |
Definition at line 226 of file sphere_cylinder-inl.h.
template bool fcl::detail::sphereCylinderIntersect | ( | const Sphere< double > & | sphere, |
const Transform3< double > & | X_FS, | ||
const Cylinder< double > & | cylinder, | ||
const Transform3< double > & | X_FC, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
FCL_EXPORT bool fcl::detail::sphereCylinderIntersect | ( | const Sphere< S > & | sphere, |
const Transform3< S > & | X_FS, | ||
const Cylinder< S > & | cylinder, | ||
const Transform3< S > & | X_FC, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Detect collision between the sphere and cylinder. If colliding, return characterization of collision in the provided vector.
The reported depth is guaranteed to be continuous with respect to the relative pose. In contrast, the normal and contact position are only guaranteed to be similarly continuous while the sphere center lies outside the cylinder. However, if the sphere center lies inside the cylinder, there are regions of discontinuity in both normal and contact position. This is due to the fact that there is not necessarily a unique characterization of penetration depth (e.g., the sphere center may be equidistant to both a cap face as well as the barrel). This ambiguity is resolved through an arbitrary prioritization scheme. If the sphere center is equidistant to both an end face and the barrel, the end face is used for normal and contact position computation. If the sphere center is closer to the barrel, but there is no unique solution (i.e., the sphere center lies on the center axis), the sphere is arbitrarily considered to be penetrating from the direction of the cylinder's +x direction (yielding a contact normal in the cylinder's -x direction.)
sphere | The sphere geometry. |
X_FS | The pose of the sphere S in the common frame F. |
cylinder | The cylinder geometry. |
X_FC | The pose of the cylinder C in the common frame F. |
contacts[out] | (optional) If the shapes collide, the contact point data will be appended to the end of this vector. |
S | The scalar parameter (must be a valid Eigen scalar). |
Definition at line 114 of file sphere_cylinder-inl.h.
template bool fcl::detail::sphereHalfspaceIntersect | ( | const Sphere< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Halfspace< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
FCL_EXPORT bool fcl::detail::sphereHalfspaceIntersect | ( | const Sphere< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 155 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
bool fcl::detail::sphereHalfspaceIntersect | ( | const Sphere< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Halfspace< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 155 of file narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h.
template bool fcl::detail::spherePlaneIntersect | ( | const Sphere< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Plane< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
FCL_EXPORT bool fcl::detail::spherePlaneIntersect | ( | const Sphere< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 122 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
bool fcl::detail::spherePlaneIntersect | ( | const Sphere< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Plane< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 122 of file narrowphase/detail/primitive_shape_algorithm/plane-inl.h.
template bool fcl::detail::sphereSphereDistance | ( | const Sphere< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Sphere< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
double * | dist, | ||
Vector3< double > * | p1, | ||
Vector3< double > * | p2 | ||
) |
bool fcl::detail::sphereSphereDistance | ( | const Sphere< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Sphere< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
S * | dist, | ||
Vector3< S > * | p1, | ||
Vector3< S > * | p2 | ||
) |
Definition at line 91 of file sphere_sphere-inl.h.
FCL_EXPORT bool fcl::detail::sphereSphereDistance | ( | const Sphere< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Sphere< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
S * | dist, | ||
Vector3< S > * | p1, | ||
Vector3< S > * | p2 | ||
) |
Definition at line 91 of file sphere_sphere-inl.h.
template bool fcl::detail::sphereSphereIntersect | ( | const Sphere< double > & | s1, |
const Transform3< double > & | tf1, | ||
const Sphere< double > & | s2, | ||
const Transform3< double > & | tf2, | ||
std::vector< ContactPoint< double >> * | contacts | ||
) |
bool fcl::detail::sphereSphereIntersect | ( | const Sphere< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Sphere< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 66 of file sphere_sphere-inl.h.
FCL_EXPORT bool fcl::detail::sphereSphereIntersect | ( | const Sphere< S > & | s1, |
const Transform3< S > & | tf1, | ||
const Sphere< S > & | s2, | ||
const Transform3< S > & | tf2, | ||
std::vector< ContactPoint< S >> * | contacts | ||
) |
Definition at line 66 of file sphere_sphere-inl.h.
|
static |
Definition at line 2314 of file gjk_libccd-inl.h.
template bool fcl::detail::sphereTriangleDistance | ( | const Sphere< double > & | sp, |
const Transform3< double > & | tf, | ||
const Vector3< double > & | P1, | ||
const Vector3< double > & | P2, | ||
const Vector3< double > & | P3, | ||
double * | dist | ||
) |
template bool fcl::detail::sphereTriangleDistance | ( | const Sphere< double > & | sp, |
const Transform3< double > & | tf, | ||
const Vector3< double > & | P1, | ||
const Vector3< double > & | P2, | ||
const Vector3< double > & | P3, | ||
double * | dist, | ||
Vector3< double > * | p1, | ||
Vector3< double > * | p2 | ||
) |
template bool fcl::detail::sphereTriangleDistance | ( | const Sphere< double > & | sp, |
const Transform3< double > & | tf1, | ||
const Vector3< double > & | P1, | ||
const Vector3< double > & | P2, | ||
const Vector3< double > & | P3, | ||
const Transform3< double > & | tf2, | ||
double * | dist, | ||
Vector3< double > * | p1, | ||
Vector3< double > * | p2 | ||
) |
FCL_EXPORT bool fcl::detail::sphereTriangleDistance | ( | const Sphere< S > & | sp, |
const Transform3< S > & | tf, | ||
const Vector3< S > & | P1, | ||
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3, | ||
S * | dist | ||
) |
Definition at line 225 of file sphere_triangle-inl.h.
bool fcl::detail::sphereTriangleDistance | ( | const Sphere< S > & | sp, |
const Transform3< S > & | tf, | ||
const Vector3< S > & | P1, | ||
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3, | ||
S * | dist | ||
) |
Definition at line 225 of file sphere_triangle-inl.h.
FCL_EXPORT bool fcl::detail::sphereTriangleDistance | ( | const Sphere< S > & | sp, |
const Transform3< S > & | tf, | ||
const Vector3< S > & | P1, | ||
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3, | ||
S * | dist, | ||
Vector3< S > * | p1, | ||
Vector3< S > * | p2 | ||
) |
Definition at line 468 of file sphere_triangle-inl.h.
bool fcl::detail::sphereTriangleDistance | ( | const Sphere< S > & | sp, |
const Transform3< S > & | tf, | ||
const Vector3< S > & | P1, | ||
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3, | ||
S * | dist, | ||
Vector3< S > * | p1, | ||
Vector3< S > * | p2 | ||
) |
Definition at line 468 of file sphere_triangle-inl.h.
FCL_EXPORT bool fcl::detail::sphereTriangleDistance | ( | const Sphere< S > & | sp, |
const Transform3< S > & | tf1, | ||
const Vector3< S > & | P1, | ||
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3, | ||
const Transform3< S > & | tf2, | ||
S * | dist, | ||
Vector3< S > * | p1, | ||
Vector3< S > * | p2 | ||
) |
Definition at line 498 of file sphere_triangle-inl.h.
bool fcl::detail::sphereTriangleDistance | ( | const Sphere< S > & | sp, |
const Transform3< S > & | tf1, | ||
const Vector3< S > & | P1, | ||
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3, | ||
const Transform3< S > & | tf2, | ||
S * | dist, | ||
Vector3< S > * | p1, | ||
Vector3< S > * | p2 | ||
) |
Definition at line 498 of file sphere_triangle-inl.h.
template bool fcl::detail::sphereTriangleIntersect | ( | const Sphere< double > & | s, |
const Transform3< double > & | tf, | ||
const Vector3< double > & | P1, | ||
const Vector3< double > & | P2, | ||
const Vector3< double > & | P3, | ||
Vector3< double > * | contact_points, | ||
double * | penetration_depth, | ||
Vector3< double > * | normal_ | ||
) |
FCL_EXPORT bool fcl::detail::sphereTriangleIntersect | ( | const Sphere< S > & | s, |
const Transform3< S > & | tf, | ||
const Vector3< S > & | P1, | ||
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3, | ||
Vector3< S > * | contact_points, | ||
S * | penetration_depth, | ||
Vector3< S > * | normal_ | ||
) |
Definition at line 139 of file sphere_triangle-inl.h.
bool fcl::detail::sphereTriangleIntersect | ( | const Sphere< S > & | s, |
const Transform3< S > & | tf, | ||
const Vector3< S > & | P1, | ||
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3, | ||
Vector3< S > * | contact_points, | ||
S * | penetration_depth, | ||
Vector3< S > * | normal_ | ||
) |
Definition at line 139 of file sphere_triangle-inl.h.
|
inlinestatic |
Support functions
Definition at line 2340 of file gjk_libccd-inl.h.
|
inlinestatic |
Definition at line 2362 of file gjk_libccd-inl.h.
|
inlinestatic |
Definition at line 2418 of file gjk_libccd-inl.h.
|
static |
Definition at line 2494 of file gjk_libccd-inl.h.
|
inlinestatic |
Definition at line 2390 of file gjk_libccd-inl.h.
|
inlinestatic |
Definition at line 2468 of file gjk_libccd-inl.h.
|
inlinestatic |
Definition at line 2450 of file gjk_libccd-inl.h.
|
static |
Definition at line 2520 of file gjk_libccd-inl.h.
fcl::detail::TEST_F | ( | ExtractClosestPoint | , |
ExtractFrom1Simplex | |||
) |
Definition at line 385 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
fcl::detail::TEST_F | ( | ExtractClosestPoint | , |
ExtractFrom1SimplexSupport | |||
) |
Definition at line 356 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
fcl::detail::TEST_F | ( | ExtractClosestPoint | , |
ExtractFrom2Simplex | |||
) |
Definition at line 440 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
fcl::detail::TEST_F | ( | ExtractClosestPoint | , |
ExtractFrom2SimplexDegenerate | |||
) |
Definition at line 461 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
fcl::detail::TEST_F | ( | ExtractClosestPoint | , |
ExtractFrom2SimplexSupport | |||
) |
Definition at line 396 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
fcl::detail::TEST_F | ( | ExtractClosestPoint | , |
ExtractFrom3SimplesDegenerateCoincident | |||
) |
Definition at line 513 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
fcl::detail::TEST_F | ( | ExtractClosestPoint | , |
ExtractFrom3SimplesDegenerateColinear | |||
) |
Definition at line 541 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
fcl::detail::TEST_F | ( | ExtractClosestPoint | , |
ExtractFrom3Simplex | |||
) |
Definition at line 486 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
void fcl::detail::TestBoxes | ( | ) |
Definition at line 408 of file test_gjk_libccd-inl_signed_distance.cpp.
void fcl::detail::TestBoxesInFrameF | ( | const Transform3< S > & | X_WF | ) |
Definition at line 247 of file test_gjk_libccd-inl_signed_distance.cpp.
void fcl::detail::TestCollidingSphereGJKSignedDistance | ( | ) |
Definition at line 190 of file test_gjk_libccd-inl_signed_distance.cpp.
void fcl::detail::TestNonCollidingSphereGJKSignedDistance | ( | ) |
Definition at line 152 of file test_gjk_libccd-inl_signed_distance.cpp.
void fcl::detail::TestSimplexToPolytope3 | ( | ) |
Definition at line 1572 of file test_gjk_libccd-inl_epa.cpp.
void fcl::detail::TestSimplexToPolytope3InGivenFrame | ( | const Transform3< S > & | X_WF | ) |
Definition at line 1376 of file test_gjk_libccd-inl_epa.cpp.
void fcl::detail::TestSphereToSphereGJKSignedDistance | ( | S | radius1, |
S | radius2, | ||
const Vector3< S > & | p_F1, | ||
const Vector3< S > & | p_F2, | ||
S | solver_tolerance, | ||
S | test_tol, | ||
S | min_distance_expected | ||
) |
Definition at line 64 of file test_gjk_libccd-inl_signed_distance.cpp.
std::array<Vector3<ccd_real_t>, 4> fcl::detail::TetrahedronColinearVertices | ( | ) |
Definition at line 146 of file test_gjk_libccd-inl_epa.cpp.
std::array<Vector3<ccd_real_t>, 4> fcl::detail::TetrahedronSmallFaceVertices | ( | ) |
Definition at line 155 of file test_gjk_libccd-inl_epa.cpp.
void fcl::detail::ThrowDetailedConfiguration | ( | const Shape1 & | s1, |
const Transform3< S > & | X_FS1, | ||
const Shape2 & | s2, | ||
const Transform3< S > & | X_FS2, | ||
const Solver & | solver, | ||
const std::exception & | e | ||
) |
Helper class for propagating a low-level exception upwards but with configuration-specific details appended. The parameters
s1 | The first shape in the query. |
X_FS1 | The pose of the first shape in frame F. |
s2 | The second shape in the query. |
X_FS2 | The pose of the second shape in frame F. |
solver | The solver. |
e | The original exception. |
Shape1 | The type of shape for shape 1. |
Shape2 | The type of shape for shape 2. |
Solver | The solver type (with scalar type erase). |
Pose | The pose type (a Transform |
Definition at line 142 of file failed_at_this_configuration.h.
void fcl::detail::ThrowFailedAtThisConfiguration | ( | const std::string & | message, |
const char * | func, | ||
const char * | file, | ||
int | line | ||
) |
Helper function for dispatching an FailedAtThisConfiguration
. Because this exception is designed to be caught and repackaged, we lose the automatic association with file and line number. This wraps them into the message of the exception so it can be preserved in the re-wrapping.
message | The error message itself. |
func | The name of the invoking function. |
file | The name of the file associated with the exception. |
line | The line number where the exception is thrown. |
Definition at line 8 of file failed_at_this_configuration.cpp.
ccd_vec3_t fcl::detail::ToCcdVec3 | ( | const Eigen::Ref< const Vector3< S >> & | v | ) |
Definition at line 1367 of file test_gjk_libccd-inl_epa.cpp.
Vector3<S> fcl::detail::ToEigenVector | ( | const ccd_vec3_t & | v | ) |
Definition at line 1362 of file test_gjk_libccd-inl_epa.cpp.
bool fcl::detail::triangle_area_is_zero | ( | const Vector3d & | a, |
const Vector3d & | b, | ||
const Vector3d & | c | ||
) |
Definition at line 177 of file test_gjk_libccd-inl_extractClosestPoints.cpp.
bool fcl::detail::TriangleMatch | ( | const ccd_pt_face_t * | f1, |
const ccd_pt_face_t * | f2, | ||
const std::unordered_map< ccd_pt_edge_t *, ccd_pt_edge_t * > & | map_e1_to_e2 | ||
) |
Definition at line 844 of file test_gjk_libccd-inl_epa.cpp.
FCL_EXPORT void* fcl::detail::triCreateGJKObject | ( | const Vector3< S > & | P1, |
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3 | ||
) |
Definition at line 2919 of file gjk_libccd-inl.h.
void* fcl::detail::triCreateGJKObject | ( | const Vector3< S > & | P1, |
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3 | ||
) |
Definition at line 2919 of file gjk_libccd-inl.h.
FCL_EXPORT void* fcl::detail::triCreateGJKObject | ( | const Vector3< S > & | P1, |
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3, | ||
const Transform3< S > & | tf | ||
) |
Definition at line 2938 of file gjk_libccd-inl.h.
void* fcl::detail::triCreateGJKObject | ( | const Vector3< S > & | P1, |
const Vector3< S > & | P2, | ||
const Vector3< S > & | P3, | ||
const Transform3< S > & | tf | ||
) |
Definition at line 2938 of file gjk_libccd-inl.h.
template void * fcl::detail::triCreateGJKObject | ( | const Vector3d & | P1, |
const Vector3d & | P2, | ||
const Vector3d & | P3 | ||
) |
template void * fcl::detail::triCreateGJKObject | ( | const Vector3d & | P1, |
const Vector3d & | P2, | ||
const Vector3d & | P3, | ||
const Transform3d & | tf | ||
) |
|
inline |
Definition at line 2958 of file gjk_libccd-inl.h.
|
inline |
Definition at line 2913 of file gjk_libccd-inl.h.
|
inline |
Definition at line 2908 of file gjk_libccd-inl.h.
void fcl::detail::updateFrontList | ( | BVHFrontList * | front_list, |
int | b1, | ||
int | b2 | ||
) |
Add new front node into the front list.
Definition at line 54 of file BVH_front.cpp.
::testing::AssertionResult fcl::detail::ValidateRepresentation | ( | const Shape & | shape, |
const std::string & | code_string | ||
) |
Definition at line 74 of file representation_util.h.
bool fcl::detail::VertexPositionCoincide | ( | const ccd_pt_vertex_t * | v1, |
const ccd_pt_vertex_t * | v2, | ||
ccd_real_t | tol | ||
) |
Definition at line 819 of file test_gjk_libccd-inl_epa.cpp.
void fcl::detail::WriteCommaSeparated | ( | std::stringstream * | sstream, |
const Transform3< S > & | p | ||
) |
Works in conjuction with ThrowDetailedConfiguration() to format the pose in a more python-like repr() way (facilitating error reproduction). In this case, just doing comma-delimited print outs makes copying-and-pasting the error message contents into code easier.
The intention is that the matrix is printed out as:
a, b, c, d, e, f, g, h, i, j, k, l, m, n, o, p;
so, that it can easily be copied and pasted into code like this:
Transform3 X; X.matrix() << a, b, c, d, e, f, g, h, i, j, k, l, m, n, o, p;
Definition at line 109 of file failed_at_this_configuration.h.
Vector3<S> fcl::detail::z_axis | ( | const Transform3< S > & | X_AB | ) |
Definition at line 154 of file capsule_capsule-inl.h.
template class FCL_EXPORT fcl::detail::CollisionTraversalNodeBase< double > |
template class FCL_EXPORT fcl::detail::ConvertBVImpl< double, AABB< double >, AABB< double > > |
template class FCL_EXPORT fcl::detail::ConvertBVImpl< double, AABB< double >, OBB< double > > |
template class FCL_EXPORT fcl::detail::ConvertBVImpl< double, AABB< double >, RSS< double > > |
template class FCL_EXPORT fcl::detail::ConvertBVImpl< double, OBB< double >, OBB< double > > |
template class FCL_EXPORT fcl::detail::ConvertBVImpl< double, OBB< double >, RSS< double > > |
template class FCL_EXPORT fcl::detail::ConvertBVImpl< double, OBBRSS< double >, OBB< double > > |
template class FCL_EXPORT fcl::detail::ConvertBVImpl< double, OBBRSS< double >, RSS< double > > |
template class FCL_EXPORT fcl::detail::ConvertBVImpl< double, RSS< double >, OBB< double > > |
template class FCL_EXPORT fcl::detail::ConvertBVImpl< double, RSS< double >, RSS< double > > |
template class FCL_EXPORT fcl::detail::DistanceTraversalNodeBase< double > |
template class FCL_EXPORT fcl::detail::GJKInitializer< double, Box< double > > |
template class FCL_EXPORT fcl::detail::GJKInitializer< double, Capsule< double > > |
template class FCL_EXPORT fcl::detail::GJKInitializer< double, Cone< double > > |
template class FCL_EXPORT fcl::detail::GJKInitializer< double, Convex< double > > |
template class FCL_EXPORT fcl::detail::GJKInitializer< double, Cylinder< double > > |
template class FCL_EXPORT fcl::detail::GJKInitializer< double, Ellipsoid< double > > |
template class FCL_EXPORT fcl::detail::GJKInitializer< double, Sphere< double > > |
template class FCL_EXPORT fcl::detail::Intersect< double > |
class FCL_EXPORT fcl::detail::IntervalTree |
Definition at line 51 of file interval_tree_node.h.
template class FCL_EXPORT fcl::detail::IntervalTreeNode< double > |
template class FCL_EXPORT fcl::detail::MeshCollisionTraversalNodekIOS< double > |
template class FCL_EXPORT fcl::detail::MeshCollisionTraversalNodeOBB< double > |
template class FCL_EXPORT fcl::detail::MeshCollisionTraversalNodeOBBRSS< double > |
template class FCL_EXPORT fcl::detail::MeshCollisionTraversalNodeRSS< double > |
template class FCL_EXPORT fcl::detail::MeshConservativeAdvancementTraversalNodeOBBRSS< double > |
template class FCL_EXPORT fcl::detail::MeshConservativeAdvancementTraversalNodeRSS< double > |
template class FCL_EXPORT fcl::detail::MeshDistanceTraversalNodekIOS< double > |
template class FCL_EXPORT fcl::detail::MeshDistanceTraversalNodeOBBRSS< double > |
template class FCL_EXPORT fcl::detail::MeshDistanceTraversalNodeRSS< double > |
template class FCL_EXPORT fcl::detail::PolySolver< double > |
template class FCL_EXPORT fcl::detail::Project< double > |
template class FCL_EXPORT fcl::detail::TraversalNodeBase< double > |
template class FCL_EXPORT fcl::detail::TriangleDistance< double > |