38 #include <boost/test/tools/old/interface.hpp> 39 #define BOOST_TEST_MODULE FCL_NESTEROV_GJK 40 #include <boost/test/included/unit_test.hpp> 42 #include <Eigen/Geometry> 94 unsigned int max_iterations = 128;
96 GJK gjk1(max_iterations, 1e-6);
108 throw std::logic_error(
"Wrong convergence criterion type");
111 GJK gjk2(max_iterations, tol);
115 GJK gjk3(max_iterations, tol);
125 std::vector<Transform3f> transforms;
132 init_support_guess.setZero();
135 for (
size_t i = 0; i < n; ++i) {
136 mink_diff.
set(&shape0, &shape1, identity, transforms[i]);
138 GJK::Status res1 = gjk1.
evaluate(mink_diff, init_guess, init_support_guess);
141 res1 = gjk1.
evaluate(mink_diff, init_guess, init_support_guess);
142 BOOST_CHECK(res1 != GJK::Status::Failed);
145 GJK::Status res2 = gjk2.
evaluate(mink_diff, init_guess, init_support_guess);
148 res2 = gjk2.
evaluate(mink_diff, init_guess, init_support_guess);
149 BOOST_CHECK(res2 != GJK::Status::Failed);
152 GJK::Status res3 = gjk3.
evaluate(mink_diff, init_guess, init_support_guess);
155 res3 = gjk3.
evaluate(mink_diff, init_guess, init_support_guess);
156 BOOST_CHECK(res3 != GJK::Status::Failed);
166 Box box0 =
Box(0.1, 0.2, 0.3);
167 Box box1 =
Box(1.1, 1.2, 1.3);
GJKConvergenceCriterionType gjk_convergence_criterion_type
Relative or absolute.
GJKConvergenceCriterionType
Wether the convergence criterion is scaled on the norm of the solution or not.
GJKConvergenceCriterion gjk_convergence_criterion
Criterion used to stop GJK.
size_t getIterations()
Get GJK number of iterations.
void generateRandomTransforms(FCL_REAL extents[6], std::vector< Transform3f > &transforms, std::size_t n)
Generate n random transforms whose translations are constrained by extents.
void set(const ShapeBase *shape0, const ShapeBase *shape1)
void test_gjk_cv_criterion(const ShapeBase &shape0, const ShapeBase &shape1, const GJKConvergenceCriterionType cv_type)
Minkowski difference class of two shapes.
#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision)
Base class for all basic geometric shapes.
Center at zero point, axis aligned box.
Eigen::Vector2i support_func_guess_t
BOOST_AUTO_TEST_CASE(set_cv_criterion)
GJKConvergenceCriterion convergence_criterion
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
GJKConvergenceCriterion
Which convergence criterion is used to stop the algorithm (when the shapes are not in collision)...
GJKConvergenceCriterionType convergence_criterion_type
Status evaluate(const MinkowskiDiff &shape, const Vec3f &guess, const support_func_guess_t &supportHint=support_func_guess_t::Zero())
GJK algorithm, given the initial value guess.