Skip to content

Commit

Permalink
Merge pull request #305 from jcarpent/topic/gjk
Browse files Browse the repository at this point in the history 
Simplify GJKSolver settings
  • Loading branch information
@jcarpent
jcarpent authored Jun 8, 2022
2 parents 32fa5f2 + bb13f10 commit a3b131e
Show file tree
Hide file tree
Showing 5 changed files with 91 additions and 69 deletions.
2 changes: 1 addition & 1 deletion include/hpp/fcl/collision.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion include/hpp/fcl/distance.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
82 changes: 80 additions & 2 deletions include/hpp/fcl/narrowphase/narrowphase.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -43,6 +43,7 @@
#include <limits>

#include <hpp/fcl/narrowphase/gjk.h>
#include <hpp/fcl/collision_data.h>

namespace hpp {
namespace fcl {
Expand Down Expand Up @@ -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;
Expand All @@ -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<FCL_REAL>::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<FCL_REAL>::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;

Expand Down
38 changes: 5 additions & 33 deletions src/collision.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand All @@ -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;
}
Expand Down
36 changes: 4 additions & 32 deletions src/distance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand Down Expand Up @@ -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) {
Expand All @@ -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;
}
Expand Down

0 comments on commit a3b131e

Please sign in to comment.