.. _program_listing_file__tmp_ws_src_hpp-fcl_include_hpp_fcl_narrowphase_narrowphase.h: Program Listing for File narrowphase.h ====================================== |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/hpp-fcl/include/hpp/fcl/narrowphase/narrowphase.h``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Centre National de la Recherche Scientifique * All rights reserved. * Copyright (c) 2021-2022, INRIA * All rights reserved. * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef HPP_FCL_NARROWPHASE_H #define HPP_FCL_NARROWPHASE_H #include #include #include #include namespace hpp { namespace fcl { struct HPP_FCL_DLLAPI GJKSolver { typedef Eigen::Array Array2d; template void initialize_gjk(details::GJK& gjk, const details::MinkowskiDiff& shape, const S1& s1, const S2& s2, Vec3f& guess, support_func_guess_t& support_hint) const { switch (gjk_initial_guess) { case GJKInitialGuess::DefaultGuess: guess = Vec3f(1, 0, 0); support_hint.setZero(); break; case GJKInitialGuess::CachedGuess: guess = cached_guess; support_hint = support_func_cached_guess; break; case GJKInitialGuess::BoundingVolumeGuess: if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) { HPP_FCL_THROW_PRETTY( "computeLocalAABB must have been called on the shapes before " "using " "GJKInitialGuess::BoundingVolumeGuess.", std::logic_error); } guess.noalias() = s1.aabb_local.center() - (shape.oR1 * s2.aabb_local.center() + shape.ot1); support_hint.setZero(); break; default: HPP_FCL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error); } // TODO: use gjk_initial_guess instead HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (enable_cached_guess) { guess = cached_guess; support_hint = support_func_cached_guess; } HPP_FCL_COMPILER_DIAGNOSTIC_POP gjk.setDistanceEarlyBreak(distance_upper_bound); gjk.gjk_variant = gjk_variant; gjk.convergence_criterion = gjk_convergence_criterion; gjk.convergence_criterion_type = gjk_convergence_criterion_type; } template bool shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const { details::MinkowskiDiff shape; shape.set(&s1, &s2, tf1, tf2); Vec3f guess; support_func_guess_t support_hint; details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance); initialize_gjk(gjk, shape, s1, s2, guess, support_hint); details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint); HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (gjk_initial_guess == GJKInitialGuess::CachedGuess || enable_cached_guess) { cached_guess = gjk.getGuessFromSimplex(); support_func_cached_guess = gjk.support_hint; } HPP_FCL_COMPILER_DIAGNOSTIC_POP Vec3f w0, w1; switch (gjk_status) { case details::GJK::Inside: if (!enable_penetration && contact_points == NULL && normal == NULL) return true; if (gjk.hasPenetrationInformation(shape)) { gjk.getClosestPoints(shape, w0, w1); distance_lower_bound = gjk.distance; if (normal) (*normal).noalias() = tf1.getRotation() * (w0 - w1).normalized(); if (contact_points) *contact_points = tf1.transform((w0 + w1) / 2); return true; } else { details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance); details::EPA::Status epa_status = epa.evaluate(gjk, -guess); if (epa_status & details::EPA::Valid || epa_status == details::EPA::OutOfFaces // Warnings || epa_status == details::EPA::OutOfVertices // Warnings ) { epa.getClosestPoints(shape, w0, w1); distance_lower_bound = -epa.depth; if (normal) (*normal).noalias() = tf1.getRotation() * epa.normal; if (contact_points) *contact_points = tf1.transform(w0 - epa.normal * (epa.depth * 0.5)); return true; } else if (epa_status == details::EPA::FallBack) { epa.getClosestPoints(shape, w0, w1); distance_lower_bound = -epa.depth; // Should be zero if (normal) (*normal).noalias() = tf1.getRotation() * epa.normal; if (contact_points) *contact_points = tf1.transform(w0); return true; } distance_lower_bound = -(std::numeric_limits::max)(); // EPA failed but we know there is a collision so we should return true; } break; case details::GJK::Valid: distance_lower_bound = gjk.distance; break; default:; } return false; } template bool shapeTriangleInteraction(const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const { bool col; // Express everything in frame 1 const Transform3f tf_1M2(tf1.inverseTimes(tf2)); TriangleP tri(tf_1M2.transform(P1), tf_1M2.transform(P2), tf_1M2.transform(P3)); details::MinkowskiDiff shape; shape.set(&s, &tri); Vec3f guess; support_func_guess_t support_hint; details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance); initialize_gjk(gjk, shape, s, tri, guess, support_hint); details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint); HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (gjk_initial_guess == GJKInitialGuess::CachedGuess || enable_cached_guess) { cached_guess = gjk.getGuessFromSimplex(); support_func_cached_guess = gjk.support_hint; } HPP_FCL_COMPILER_DIAGNOSTIC_PUSH Vec3f w0, w1; switch (gjk_status) { case details::GJK::Inside: col = true; if (gjk.hasPenetrationInformation(shape)) { gjk.getClosestPoints(shape, w0, w1); distance = gjk.distance; normal.noalias() = tf1.getRotation() * (w0 - w1).normalized(); p1 = p2 = tf1.transform((w0 + w1) / 2); } else { details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance); details::EPA::Status epa_status = epa.evaluate(gjk, -guess); if (epa_status & details::EPA::Valid || epa_status == details::EPA::OutOfFaces // Warnings || epa_status == details::EPA::OutOfVertices // Warnings ) { epa.getClosestPoints(shape, w0, w1); distance = -epa.depth; normal.noalias() = tf1.getRotation() * epa.normal; p1 = p2 = tf1.transform(w0 - epa.normal * (epa.depth * 0.5)); assert(distance <= 1e-6); } else { distance = -(std::numeric_limits::max)(); gjk.getClosestPoints(shape, w0, w1); p1 = p2 = tf1.transform(w0); } } break; case details::GJK::Valid: case details::GJK::EarlyStopped: case details::GJK::Failed: col = false; gjk.getClosestPoints(shape, p1, p2); // TODO On degenerated case, the closest point may be wrong // (i.e. an object face normal is colinear to gjk.ray // assert (distance == (w0 - w1).norm()); distance = gjk.distance; p1 = tf1.transform(p1); p2 = tf1.transform(p2); assert(distance > 0); break; default: assert(false && "should not reach type part."); throw std::logic_error("GJKSolver: should not reach this part."); } return col; } template bool shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const { #ifndef NDEBUG FCL_REAL eps(sqrt(std::numeric_limits::epsilon())); #endif details::MinkowskiDiff shape; shape.set(&s1, &s2, tf1, tf2); Vec3f guess; support_func_guess_t support_hint; details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance); initialize_gjk(gjk, shape, s1, s2, guess, support_hint); details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint); if (gjk_initial_guess == GJKInitialGuess::CachedGuess || enable_cached_guess) { cached_guess = gjk.getGuessFromSimplex(); support_func_cached_guess = gjk.support_hint; } if (gjk_status == details::GJK::Failed) { // TODO: understand why GJK fails between cylinder and box assert(distance * distance < sqrt(eps)); Vec3f w0, w1; gjk.getClosestPoints(shape, w0, w1); distance = 0; p1 = tf1.transform(w0); p2 = tf1.transform(w1); normal.setZero(); return false; } else if (gjk_status == details::GJK::Valid) { gjk.getClosestPoints(shape, p1, p2); // TODO On degenerated case, the closest point may be wrong // (i.e. an object face normal is colinear to gjk.ray // assert (distance == (w0 - w1).norm()); distance = gjk.distance; normal.noalias() = tf1.getRotation() * gjk.ray; normal.normalize(); p1 = tf1.transform(p1); p2 = tf1.transform(p2); return true; } else if (gjk_status == details::GJK::EarlyStopped) { distance = gjk.distance; p1 = p2 = normal = Vec3f::Constant(std::numeric_limits::quiet_NaN()); return true; } else { assert(gjk_status == details::GJK::Inside); if (gjk.hasPenetrationInformation(shape)) { gjk.getClosestPoints(shape, p1, p2); distance = gjk.distance; // Return contact points in case of collision // p1 = tf1.transform (p1); // p2 = tf1.transform (p2); normal.noalias() = tf1.getRotation() * (p1 - p2); normal.normalize(); p1 = tf1.transform(p1); p2 = tf1.transform(p2); } else { details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance); details::EPA::Status epa_status = epa.evaluate(gjk, -guess); if (epa_status & details::EPA::Valid || epa_status == details::EPA::OutOfFaces // Warnings || epa_status == details::EPA::OutOfVertices // Warnings || epa_status == details::EPA::FallBack) { Vec3f w0, w1; epa.getClosestPoints(shape, w0, w1); assert(epa.depth >= -eps); distance = (std::min)(0., -epa.depth); normal.noalias() = tf1.getRotation() * epa.normal; p1 = tf1.transform(w0); p2 = tf1.transform(w1); return false; } distance = -(std::numeric_limits::max)(); gjk.getClosestPoints(shape, p1, p2); p1 = tf1.transform(p1); p2 = tf1.transform(p2); } return false; } } HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS GJKSolver() { gjk_max_iterations = 128; gjk_tolerance = 1e-6; epa_max_face_num = 128; epa_max_vertex_num = 64; epa_max_iterations = 255; epa_tolerance = 1e-6; enable_cached_guess = false; // TODO: use gjk_initial_guess instead cached_guess = Vec3f(1, 0, 0); support_func_cached_guess = support_func_guess_t::Zero(); distance_upper_bound = (std::numeric_limits::max)(); gjk_initial_guess = GJKInitialGuess::DefaultGuess; gjk_variant = GJKVariant::DefaultGJK; gjk_convergence_criterion = GJKConvergenceCriterion::VDB; gjk_convergence_criterion_type = GJKConvergenceCriterionType::Relative; } GJKSolver(const DistanceRequest& request) { cached_guess = Vec3f(1, 0, 0); support_func_cached_guess = support_func_guess_t::Zero(); distance_upper_bound = (std::numeric_limits::max)(); // EPS settings epa_max_face_num = 128; epa_max_vertex_num = 64; epa_max_iterations = 255; epa_tolerance = 1e-6; set(request); } void set(const DistanceRequest& request) { gjk_initial_guess = request.gjk_initial_guess; // TODO: use gjk_initial_guess instead enable_cached_guess = request.enable_cached_gjk_guess; gjk_variant = request.gjk_variant; gjk_convergence_criterion = request.gjk_convergence_criterion; gjk_convergence_criterion_type = request.gjk_convergence_criterion_type; gjk_tolerance = request.gjk_tolerance; gjk_max_iterations = request.gjk_max_iterations; if (gjk_initial_guess == GJKInitialGuess::CachedGuess || enable_cached_guess) { cached_guess = request.cached_gjk_guess; support_func_cached_guess = request.cached_support_func_guess; } } GJKSolver(const CollisionRequest& request) { cached_guess = Vec3f(1, 0, 0); support_func_cached_guess = support_func_guess_t::Zero(); distance_upper_bound = (std::numeric_limits::max)(); // EPS settings epa_max_face_num = 128; epa_max_vertex_num = 64; epa_max_iterations = 255; epa_tolerance = 1e-6; set(request); } void set(const CollisionRequest& request) { gjk_initial_guess = request.gjk_initial_guess; // TODO: use gjk_initial_guess instead enable_cached_guess = request.enable_cached_gjk_guess; gjk_variant = request.gjk_variant; gjk_convergence_criterion = request.gjk_convergence_criterion; gjk_convergence_criterion_type = request.gjk_convergence_criterion_type; gjk_tolerance = request.gjk_tolerance; gjk_max_iterations = request.gjk_max_iterations; if (gjk_initial_guess == GJKInitialGuess::CachedGuess || enable_cached_guess) { cached_guess = request.cached_gjk_guess; support_func_cached_guess = request.cached_support_func_guess; } // The distance upper bound should be at least greater to the requested // security margin. Otherwise, we will likely miss some collisions. distance_upper_bound = (std::max)( 0., (std::max)(request.distance_upper_bound, request.security_margin)); } GJKSolver(const GJKSolver& other) = default; HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS bool operator==(const GJKSolver& other) const { return epa_max_face_num == other.epa_max_face_num && epa_max_vertex_num == other.epa_max_vertex_num && epa_max_iterations == other.epa_max_iterations && epa_tolerance == other.epa_tolerance && gjk_max_iterations == other.gjk_max_iterations && enable_cached_guess == other.enable_cached_guess && // TODO: use gjk_initial_guess // instead cached_guess == other.cached_guess && support_func_cached_guess == other.support_func_cached_guess && distance_upper_bound == other.distance_upper_bound && gjk_initial_guess == other.gjk_initial_guess && gjk_variant == other.gjk_variant && gjk_convergence_criterion == other.gjk_convergence_criterion && gjk_convergence_criterion_type == other.gjk_convergence_criterion_type; } HPP_FCL_COMPILER_DIAGNOSTIC_POP bool operator!=(const GJKSolver& other) const { return !(*this == other); } unsigned int epa_max_face_num; unsigned int epa_max_vertex_num; unsigned int epa_max_iterations; FCL_REAL epa_tolerance; mutable FCL_REAL gjk_tolerance; mutable size_t gjk_max_iterations; HPP_FCL_DEPRECATED_MESSAGE("Use gjk_initial_guess instead") mutable bool enable_cached_guess; mutable Vec3f cached_guess; mutable GJKInitialGuess gjk_initial_guess; mutable GJKVariant gjk_variant; mutable GJKConvergenceCriterion gjk_convergence_criterion; mutable GJKConvergenceCriterionType gjk_convergence_criterion_type; mutable support_func_guess_t support_func_cached_guess; mutable FCL_REAL distance_upper_bound; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; template <> HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction( const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const; template <> HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction( const Halfspace& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const; template <> HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction( const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const; #define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2) \ template <> \ HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect( \ const S1& s1, const Transform3f& tf1, const S2& s2, \ const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool, \ Vec3f* contact_points, Vec3f* normal) const #define SHAPE_INTERSECT_SPECIALIZATION(S1, S2) \ SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2); \ SHAPE_INTERSECT_SPECIALIZATION_BASE(S2, S1) SHAPE_INTERSECT_SPECIALIZATION(Sphere, Capsule); SHAPE_INTERSECT_SPECIALIZATION_BASE(Sphere, Sphere); SHAPE_INTERSECT_SPECIALIZATION(Sphere, Box); SHAPE_INTERSECT_SPECIALIZATION(Sphere, Halfspace); SHAPE_INTERSECT_SPECIALIZATION(Sphere, Plane); SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Box); SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Capsule); SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Cylinder); SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Cone); SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Plane); SHAPE_INTERSECT_SPECIALIZATION(Plane, Box); SHAPE_INTERSECT_SPECIALIZATION(Plane, Capsule); SHAPE_INTERSECT_SPECIALIZATION(Plane, Cylinder); SHAPE_INTERSECT_SPECIALIZATION(Plane, Cone); #undef SHAPE_INTERSECT_SPECIALIZATION #undef SHAPE_INTERSECT_SPECIALIZATION_BASE #define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2) \ template <> \ HPP_FCL_DLLAPI bool GJKSolver::shapeDistance( \ const S1& s1, const Transform3f& tf1, const S2& s2, \ const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \ Vec3f& normal) const #define SHAPE_DISTANCE_SPECIALIZATION(S1, S2) \ SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2); \ SHAPE_DISTANCE_SPECIALIZATION_BASE(S2, S1) SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule); SHAPE_DISTANCE_SPECIALIZATION(Sphere, Box); SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder); SHAPE_DISTANCE_SPECIALIZATION_BASE(Sphere, Sphere); SHAPE_DISTANCE_SPECIALIZATION_BASE(Capsule, Capsule); SHAPE_DISTANCE_SPECIALIZATION_BASE(TriangleP, TriangleP); #undef SHAPE_DISTANCE_SPECIALIZATION #undef SHAPE_DISTANCE_SPECIALIZATION_BASE #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wc99-extensions" #endif // param doc is the doxygen detailled description (should be enclosed in /** */ // and contain no dot for some obscure reasons). #define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc) \ \ doc template <> \ HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect( \ const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \ const Transform3f& tf2, FCL_REAL& distance_lower_bound, \ bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const #define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape, doc) \ HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape, Shape, doc) #define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1, Shape2, doc) \ HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc); \ HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2, Shape1, doc) HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Sphere, ); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule, ); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Halfspace, ); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Plane, ); template <> HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect( const Box& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const; #ifdef IS_DOXYGEN // for doxygen only template <> HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect( const Box& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const; #endif // HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Halfspace, ); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Plane, ); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Halfspace, ); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Plane, ); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Halfspace, ); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Plane, ); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Halfspace, ); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Plane, ); HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Halfspace, ); HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Plane, ); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Plane, Halfspace, ); #undef HPP_FCL_DECLARE_SHAPE_INTERSECT #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF #undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR // param doc is the doxygen detailled description (should be enclosed in /** */ // and contain no dot for some obscure reasons). #define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape, doc) \ \ doc template <> \ HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction( \ const Shape& s, const Transform3f& tf1, const Vec3f& P1, \ const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, \ FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const HPP_FCL_DECLARE_SHAPE_TRIANGLE(Sphere, ); HPP_FCL_DECLARE_SHAPE_TRIANGLE(Halfspace, ); HPP_FCL_DECLARE_SHAPE_TRIANGLE(Plane, ); #undef HPP_FCL_DECLARE_SHAPE_TRIANGLE // param doc is the doxygen detailled description (should be enclosed in /** */ // and contain no dot for some obscure reasons). #define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc) \ \ doc template <> \ bool HPP_FCL_DLLAPI GJKSolver::shapeDistance( \ const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \ const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \ Vec3f& normal) const #define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape, doc) \ HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape, Shape, doc) #define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1, Shape2, doc) \ HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc); \ HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2, Shape1, doc) HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Box, ); HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Capsule, ); HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Cylinder, ); HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Sphere, ); HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF( Capsule, ); HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF( TriangleP, ); #undef HPP_FCL_DECLARE_SHAPE_DISTANCE #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) #pragma GCC diagnostic pop #endif } // namespace fcl } // namespace hpp #endif