diff --git a/include/coal/collision_data.h b/include/coal/collision_data.h index 6e17f0485a45ac0470d281180304fbbf517b8927..e885e66a6280f70ef843b74adca968fcb03eea62 100644 --- a/include/coal/collision_data.h +++ b/include/coal/collision_data.h @@ -987,12 +987,12 @@ struct COAL_DLLAPI DistanceRequest : QueryRequest { /// Nearest points are always computed and are the points of the shapes that /// achieve a distance of `DistanceResult::min_distance`. COAL_DEPRECATED_MESSAGE( - Nearest points are always computed - : they are the points of the shapes that achieve a distance of + Nearest points are always computed : they are the points of the shapes + that achieve a distance of `DistanceResult::min_distance` - .\n Use `enable_signed_distance` if you want to compute a signed - minimum distance(and thus its corresponding nearest points) - .) + .\n Use `enable_signed_distance` if you want to compute a signed + minimum distance(and thus its corresponding nearest points) + .) bool enable_nearest_points; /// @brief whether to compute the penetration depth when objects are in diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index 9d246b48e11c4c357fcbac363afb64b9c382738f..b3afc55942ec2cfef2d3688afae70370415a3a9b 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -577,8 +577,7 @@ void exposeCollisionGeometries() { "Check whether two AABB are overlaping and return the overloaping " "part if true.") - .def("distance", - (CoalScalar(AABB::*)(const AABB&) const) & AABB::distance, + .def("distance", (CoalScalar(AABB::*)(const AABB&) const)&AABB::distance, bp::args("self", "other"), "Distance between two AABBs.") // .def("distance", // AABB_distance_proxy, @@ -643,10 +642,10 @@ void exposeCollisionGeometries() { #endif ; - def("translate", (AABB(*)(const AABB&, const Vec3s&)) & translate, + def("translate", (AABB(*)(const AABB&, const Vec3s&))&translate, bp::args("aabb", "t"), "Translate the center of AABB by t"); - def("rotate", (AABB(*)(const AABB&, const Matrix3s&)) & rotate, + def("rotate", (AABB(*)(const AABB&, const Matrix3s&))&rotate, bp::args("aabb", "R"), "Rotate the AABB object by R"); if (!eigenpy::register_symbolic_link_to_registered_type< diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp index 6ae95bcf7b1ad2beeefd97c3af89ac98d89b7264..b956fe144cccb8cc5b32d1924a1014e3cca503fe 100644 --- a/src/BV/OBB.cpp +++ b/src/BV/OBB.cpp @@ -310,29 +310,28 @@ inline CoalScalar obbDisjoint_check_B_axis(const Matrix3s& B, const Vec3s& T, } template <int ib, int jb = (ib + 1) % 3, int kb = (ib + 2) % 3> -struct COAL_LOCAL obbDisjoint_check_Ai_cross_Bi { - static inline bool run(int ia, int ja, int ka, const Matrix3s& B, - const Vec3s& T, const Vec3s& a, const Vec3s& b, - const Matrix3s& Bf, const CoalScalar& breakDistance2, - CoalScalar& squaredLowerBoundDistance) { - CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); - if (sinus2 < 1e-6) return false; - - const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - - const CoalScalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); - // We need to divide by the norm || Aia x Bib || - // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 - if (diff > 0) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } - } - return false; +struct COAL_LOCAL obbDisjoint_check_Ai_cross_Bi{static inline bool run( + int ia, int ja, int ka, const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b, const Matrix3s& Bf, const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance){CoalScalar sinus2 = + 1 - Bf(ia, ib) * Bf(ia, ib); +if (sinus2 < 1e-6) return false; + +const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); + +const CoalScalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); +// We need to divide by the norm || Aia x Bib || +// As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 +if (diff > 0) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } -}; +} +return false; +} // namespace internal +}; // namespace coal } // namespace internal // B, T orientation and position of 2nd OBB in frame of 1st OBB, diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index e30a2c510d8cd5598dde270dfdb909aaca3ff499..b654cfbda11ef51db4eafff44c04afe0eb214a00 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -97,66 +97,61 @@ BVH_SHAPE_DEFAULT_TO_ORIENTED(OBBRSS); /// - 0 if the query should be made with non-aligned object frames. template <typename T_BVH, typename T_SH, int _Options = details::bvh_shape_traits<T_BVH, T_SH>::Options> -struct COAL_LOCAL BVHShapeCollider { - static std::size_t collide(const CollisionGeometry* o1, - const Transform3s& tf1, - const CollisionGeometry* o2, - const Transform3s& tf2, const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { +struct COAL_LOCAL BVHShapeCollider{static std::size_t collide( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* nsolver, const CollisionRequest& request, + CollisionResult& result){ if (request.isSatisfied(result)) return result.numContacts(); - if (request.security_margin < 0) - COAL_THROW_PRETTY( - "Negative security margin are not handled yet for BVHModel", - std::invalid_argument); +if (request.security_margin < 0) + COAL_THROW_PRETTY("Negative security margin are not handled yet for BVHModel", + std::invalid_argument); - if (_Options & RelativeTransformationIsIdentity) - return aligned(o1, tf1, o2, tf2, nsolver, request, result); - else - return oriented(o1, tf1, o2, tf2, nsolver, request, result); - } +if (_Options & RelativeTransformationIsIdentity) + return aligned(o1, tf1, o2, tf2, nsolver, request, result); +else + return oriented(o1, tf1, o2, tf2, nsolver, request, result); +} // namespace coal - static std::size_t aligned(const CollisionGeometry* o1, - const Transform3s& tf1, - const CollisionGeometry* o2, - const Transform3s& tf2, const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - if (request.isSatisfied(result)) return result.numContacts(); +static std::size_t aligned(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { + if (request.isSatisfied(result)) return result.numContacts(); - MeshShapeCollisionTraversalNode<T_BVH, T_SH, - RelativeTransformationIsIdentity> - node(request); - const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1); - BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1); - Transform3s tf1_tmp = tf1; - const T_SH* obj2 = static_cast<const T_SH*>(o2); + MeshShapeCollisionTraversalNode<T_BVH, T_SH, RelativeTransformationIsIdentity> + node(request); + const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1); + BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1); + Transform3s tf1_tmp = tf1; + const T_SH* obj2 = static_cast<const T_SH*>(o2); - initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, result); - coal::collide(&node, request, result); + initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, result); + coal::collide(&node, request, result); - delete obj1_tmp; - return result.numContacts(); - } + delete obj1_tmp; + return result.numContacts(); +} - static std::size_t oriented(const CollisionGeometry* o1, - const Transform3s& tf1, - const CollisionGeometry* o2, - const Transform3s& tf2, const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - if (request.isSatisfied(result)) return result.numContacts(); +static std::size_t oriented(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { + if (request.isSatisfied(result)) return result.numContacts(); - MeshShapeCollisionTraversalNode<T_BVH, T_SH, 0> node(request); - const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1); - const T_SH* obj2 = static_cast<const T_SH*>(o2); + MeshShapeCollisionTraversalNode<T_BVH, T_SH, 0> node(request); + const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1); + const T_SH* obj2 = static_cast<const T_SH*>(o2); - initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result); - coal::collide(&node, request, result); - return result.numContacts(); - } -}; + initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result); + coal::collide(&node, request, result); + return result.numContacts(); +} +} +; /// @brief Collider functor for HeightField data structure /// \tparam _Options takes two values. diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp index fbfeb664050bb1ce607a6d14c9d93e7b8216e103..d9aae3ba3d5c27327c341aa9ebdfba12d4e582e2 100644 --- a/src/distance_func_matrix.cpp +++ b/src/distance_func_matrix.cpp @@ -83,27 +83,26 @@ COAL_LOCAL CoalScalar distance_function_not_implemented( } template <typename T_BVH, typename T_SH> -struct COAL_LOCAL BVHShapeDistancer { - static CoalScalar distance(const CollisionGeometry* o1, - const Transform3s& tf1, - const CollisionGeometry* o2, - const Transform3s& tf2, const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { +struct COAL_LOCAL BVHShapeDistancer{static CoalScalar distance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* nsolver, const DistanceRequest& request, + DistanceResult& result){ if (request.isSatisfied(result)) return result.min_distance; - MeshShapeDistanceTraversalNode<T_BVH, T_SH> node; - const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1); - BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1); - Transform3s tf1_tmp = tf1; - const T_SH* obj2 = static_cast<const T_SH*>(o2); +MeshShapeDistanceTraversalNode<T_BVH, T_SH> node; +const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1); +BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1); +Transform3s tf1_tmp = tf1; +const T_SH* obj2 = static_cast<const T_SH*>(o2); - initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result); - ::coal::distance(&node); +initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result); +::coal::distance(&node); - delete obj1_tmp; - return result.min_distance; - } -}; +delete obj1_tmp; +return result.min_distance; +} // namespace coal +} +; namespace details { @@ -172,35 +171,33 @@ struct COAL_LOCAL BVHShapeDistancer<OBBRSS, T_SH> { }; template <typename T_HF, typename T_SH> -struct COAL_LOCAL HeightFieldShapeDistancer { - static CoalScalar distance(const CollisionGeometry* o1, - const Transform3s& tf1, - const CollisionGeometry* o2, - const Transform3s& tf2, const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { - COAL_UNUSED_VARIABLE(o1); - COAL_UNUSED_VARIABLE(tf1); - COAL_UNUSED_VARIABLE(o2); - COAL_UNUSED_VARIABLE(tf2); - COAL_UNUSED_VARIABLE(nsolver); - COAL_UNUSED_VARIABLE(request); - // TODO(jcarpent) - COAL_THROW_PRETTY( - "Distance between a height field and a shape is not implemented", - std::invalid_argument); - // if(request.isSatisfied(result)) return result.min_distance; - // HeightFieldShapeDistanceTraversalNode<T_HF, T_SH> node; - // - // const HeightField<T_HF>* obj1 = static_cast<const HeightField<T_HF>* - // >(o1); const T_SH* obj2 = static_cast<const T_SH*>(o2); - // - // initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); - // fcl::distance(&node); - - return result.min_distance; - } -}; +struct COAL_LOCAL HeightFieldShapeDistancer{static CoalScalar distance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* nsolver, const DistanceRequest& request, + DistanceResult& result){COAL_UNUSED_VARIABLE(o1); +COAL_UNUSED_VARIABLE(tf1); +COAL_UNUSED_VARIABLE(o2); +COAL_UNUSED_VARIABLE(tf2); +COAL_UNUSED_VARIABLE(nsolver); +COAL_UNUSED_VARIABLE(request); +// TODO(jcarpent) +COAL_THROW_PRETTY( + "Distance between a height field and a shape is not implemented", + std::invalid_argument); +// if(request.isSatisfied(result)) return result.min_distance; +// HeightFieldShapeDistanceTraversalNode<T_HF, T_SH> node; +// +// const HeightField<T_HF>* obj1 = static_cast<const HeightField<T_HF>* +// >(o1); const T_SH* obj2 = static_cast<const T_SH*>(o2); +// +// initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); +// fcl::distance(&node); + +return result.min_distance; +} +} +; template <typename T_BVH> CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1, diff --git a/src/traits_traversal.h b/src/traits_traversal.h index 8666d34a2aae448c7709d9f57a1fd19f4584c21b..5667a69c67a6db3fa8ea185e3f24434d38339eb6 100644 --- a/src/traits_traversal.h +++ b/src/traits_traversal.h @@ -18,7 +18,7 @@ namespace coal { // TraversalTraitsCollision for collision_func_matrix.cpp template <typename TypeA, typename TypeB> -struct COAL_LOCAL TraversalTraitsCollision {}; +struct COAL_LOCAL TraversalTraitsCollision{}; #ifdef COAL_HAS_OCTOMAP @@ -62,7 +62,7 @@ struct COAL_LOCAL TraversalTraitsCollision<HeightField<T_HF>, OcTree> { // TraversalTraitsDistance for distance_func_matrix.cpp template <typename TypeA, typename TypeB> -struct COAL_LOCAL TraversalTraitsDistance {}; +struct COAL_LOCAL TraversalTraitsDistance{}; #ifdef COAL_HAS_OCTOMAP diff --git a/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp index 909e2979b1b30c857a6a6fda4bec5602d3894e19..2b133c5a6487d1fa7bfda8cbaf3936f0ddf0cb33 100644 --- a/src/traversal/traversal_recurse.cpp +++ b/src/traversal/traversal_recurse.cpp @@ -211,11 +211,11 @@ struct COAL_LOCAL BVT { }; /** @brief Comparer between two BVT */ -struct COAL_LOCAL BVT_Comparer { - bool operator()(const BVT& lhs, const BVT& rhs) const { - return lhs.d > rhs.d; - } -}; +struct COAL_LOCAL BVT_Comparer{bool operator()(const BVT& lhs, const BVT& rhs) + const {return lhs.d > rhs.d; +} // namespace coal +} +; struct COAL_LOCAL BVTQ { BVTQ() : qsize(2) {}