From 7179d04a5a2698191e7c54fd0a0c97a7986da159 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 7 Jun 2022 23:12:35 +0200 Subject: [PATCH 1/3] core: set mutable GJKSolver --- include/hpp/fcl/collision.h | 2 +- include/hpp/fcl/distance.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/hpp/fcl/collision.h b/include/hpp/fcl/collision.h index 092c88ae3..e25f3ae27 100644 --- a/include/hpp/fcl/collision.h +++ b/include/hpp/fcl/collision.h @@ -136,7 +136,7 @@ class HPP_FCL_DLLAPI ComputeCollision { mutable const CollisionGeometry* o1; mutable const CollisionGeometry* o2; - GJKSolver solver; + mutable GJKSolver solver; CollisionFunctionMatrix::CollisionFunc func; bool swap_geoms; diff --git a/include/hpp/fcl/distance.h b/include/hpp/fcl/distance.h index c3e7b2fda..a146508c2 100644 --- a/include/hpp/fcl/distance.h +++ b/include/hpp/fcl/distance.h @@ -131,7 +131,7 @@ class HPP_FCL_DLLAPI ComputeDistance { mutable const CollisionGeometry* o1; mutable const CollisionGeometry* o2; - GJKSolver solver; + mutable GJKSolver solver; DistanceFunctionMatrix::DistanceFunc func; bool swap_geoms; From 59ee1a1174864b47517c34e44abadda50391a435 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 7 Jun 2022 23:15:03 +0200 Subject: [PATCH 2/3] gjk: add setter from {Collision,Distance}Request --- include/hpp/fcl/narrowphase/narrowphase.h | 82 ++++++++++++++++++++++- 1 file changed, 80 insertions(+), 2 deletions(-) diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index f749118c7..f63bb0fd2 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -5,7 +5,7 @@ * Copyright (c) 2014-2015, Open Source Robotics Foundation * Copyright (c) 2018-2019, Centre National de la Recherche Scientifique * All rights reserved. - * Copyright (c) 2021, INRIA + * 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 @@ -43,6 +43,7 @@ #include #include +#include namespace hpp { namespace fcl { @@ -329,7 +330,7 @@ struct HPP_FCL_DLLAPI GJKSolver { HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - /// @brief default setting for GJK algorithm + /// @brief Default constructor for GJK algorithm GJKSolver() { gjk_max_iterations = 128; gjk_tolerance = 1e-6; @@ -347,6 +348,83 @@ struct HPP_FCL_DLLAPI GJKSolver { gjk_convergence_criterion_type = GJKConvergenceCriterionType::Relative; } + /// @brief Constructor from a DistanceRequest + /// + /// \param[in] request DistanceRequest input + /// + 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); + } + + /// @brief setter from a DistanceRequest + /// + /// \param[in] request DistanceRequest input + /// + 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; + } + } + + /// @brief Constructor from a CollisionRequest + /// + /// \param[in] request CollisionRequest input + /// + 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); + } + + /// @brief setter from a CollisionRequest + /// + /// \param[in] request CollisionRequest input + /// + 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; + } + distance_upper_bound = request.distance_upper_bound; + } + /// @brief Copy constructor GJKSolver(const GJKSolver& other) = default; From bb13f10b1cafa6879c0fc05af7a159776d3217ae Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 7 Jun 2022 23:15:35 +0200 Subject: [PATCH 3/3] core: simplify setup of GJKSolver --- src/collision.cpp | 38 +++++--------------------------------- src/distance.cpp | 36 ++++-------------------------------- 2 files changed, 9 insertions(+), 65 deletions(-) diff --git a/src/collision.cpp b/src/collision.cpp index 1be85f6a6..3da3e4af5 100644 --- a/src/collision.cpp +++ b/src/collision.cpp @@ -75,21 +75,8 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, result.clear(); return false; } - GJKSolver solver; - solver.gjk_initial_guess = request.gjk_initial_guess; - // TODO: use gjk_initial_guess instead - solver.enable_cached_guess = request.enable_cached_gjk_guess; - solver.gjk_variant = request.gjk_variant; - solver.gjk_convergence_criterion = request.gjk_convergence_criterion; - solver.gjk_convergence_criterion_type = - request.gjk_convergence_criterion_type; - solver.gjk_tolerance = request.gjk_tolerance; - solver.gjk_max_iterations = request.gjk_max_iterations; - if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess || - solver.enable_cached_guess) { - solver.cached_guess = request.cached_gjk_guess; - solver.support_func_cached_guess = request.cached_support_func_guess; - } + + GJKSolver solver(request); const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable(); std::size_t res; @@ -194,23 +181,7 @@ std::size_t ComputeCollision::operator()(const Transform3f& tf1, CollisionResult& result) const { - GJKInitialGuess gjk_initial_guess = request.gjk_initial_guess; - solver.gjk_initial_guess = gjk_initial_guess; - bool cached = (gjk_initial_guess == GJKInitialGuess::CachedGuess || - request.enable_cached_gjk_guess); - solver.enable_cached_guess = cached; - if (cached) { - solver.cached_guess = request.cached_gjk_guess; - solver.support_func_cached_guess = request.cached_support_func_guess; - } - - solver.distance_upper_bound = request.distance_upper_bound; - solver.gjk_tolerance = request.gjk_tolerance; - solver.gjk_max_iterations = request.gjk_max_iterations; - solver.gjk_variant = request.gjk_variant; - solver.gjk_convergence_criterion = request.gjk_convergence_criterion; - solver.gjk_convergence_criterion_type = - request.gjk_convergence_criterion_type; + solver.set(request); std::size_t res; if (request.enable_timings) { @@ -220,7 +191,8 @@ std::size_t ComputeCollision::operator()(const Transform3f& tf1, } else res = run(tf1, tf2, request, result); - if (cached) { + if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess || + solver.enable_cached_guess) { result.cached_gjk_guess = solver.cached_guess; result.cached_support_func_guess = solver.support_func_cached_guess; } diff --git a/src/distance.cpp b/src/distance.cpp index 50030c760..c9625c6c5 100644 --- a/src/distance.cpp +++ b/src/distance.cpp @@ -60,21 +60,7 @@ FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result) { - GJKSolver solver; - solver.gjk_initial_guess = request.gjk_initial_guess; - // TODO: use gjk_initial_guess instead - solver.enable_cached_guess = request.enable_cached_gjk_guess; - solver.gjk_variant = request.gjk_variant; - solver.gjk_convergence_criterion = request.gjk_convergence_criterion; - solver.gjk_convergence_criterion_type = - request.gjk_convergence_criterion_type; - solver.gjk_tolerance = request.gjk_tolerance; - solver.gjk_max_iterations = request.gjk_max_iterations; - if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess || - solver.enable_cached_guess) { - solver.cached_guess = request.cached_gjk_guess; - solver.support_func_cached_guess = request.cached_support_func_guess; - } + GJKSolver solver(request); const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable(); @@ -179,22 +165,7 @@ FCL_REAL ComputeDistance::operator()(const Transform3f& tf1, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result) const { - GJKInitialGuess gjk_initial_guess = request.gjk_initial_guess; - solver.gjk_initial_guess = gjk_initial_guess; - bool cached = (gjk_initial_guess == GJKInitialGuess::CachedGuess || - request.enable_cached_gjk_guess); - solver.enable_cached_guess = cached; - if (cached) { - solver.cached_guess = request.cached_gjk_guess; - solver.support_func_cached_guess = request.cached_support_func_guess; - } - - solver.gjk_tolerance = request.gjk_tolerance; - solver.gjk_max_iterations = request.gjk_max_iterations; - solver.gjk_variant = request.gjk_variant; - solver.gjk_convergence_criterion = request.gjk_convergence_criterion; - solver.gjk_convergence_criterion_type = - request.gjk_convergence_criterion_type; + solver.set(request); FCL_REAL res; if (request.enable_timings) { @@ -204,7 +175,8 @@ FCL_REAL ComputeDistance::operator()(const Transform3f& tf1, } else res = run(tf1, tf2, request, result); - if (cached) { + if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess || + solver.enable_cached_guess) { result.cached_gjk_guess = solver.cached_guess; result.cached_support_func_guess = solver.support_func_cached_guess; }