diff --git a/doc/manual/manual/contractors/shape/index.rst b/doc/manual/manual/contractors/shape/index.rst index 793066cca..b2e377fdd 100644 --- a/doc/manual/manual/contractors/shape/index.rst +++ b/doc/manual/manual/contractors/shape/index.rst @@ -1,5 +1,5 @@ -Geometric contractors -===================== +Shape contractors +================= .. toctree:: diff --git a/doc/manual/manual/tools/registration.rst b/doc/manual/manual/tools/registration.rst index c2c89e978..2d22ba1ad 100644 --- a/doc/manual/manual/tools/registration.rst +++ b/doc/manual/manual/tools/registration.rst @@ -72,7 +72,7 @@ The following example estimates a transformation between two sampled trajectorie dst_estim = SampledTraj_Vector() for ti,src_i in src: - dst_estim.set(ti, tr*src_i) + dst_estim.set(tr*src_i, ti) .. code-tab:: c++ @@ -108,7 +108,7 @@ The following example estimates a transformation between two sampled trajectorie SampledTraj dst_estim; for(const auto& [ti,src_i] : src) - dst_estim.set(ti, tr*Eigen::Vector2d(src_i)); + dst_estim.set(tr*Eigen::Vector2d(src_i), ti); .. figure:: registration.png diff --git a/python/src/core/separators/codac2_py_SepInter.cpp b/python/src/core/separators/codac2_py_SepInter.cpp index d650f67c1..4d2d2382d 100644 --- a/python/src/core/separators/codac2_py_SepInter.cpp +++ b/python/src/core/separators/codac2_py_SepInter.cpp @@ -24,6 +24,17 @@ void export_SepInter(py::module& m, py::class_& pysep) py::class_ exported(m, "SepInter", pysep, SEPINTER_MAIN); exported + .def(py::init( + [](const py::list& l) + { + Collection l_copy; + for(const auto& li : l) + l_copy.push_back(li.cast().copy()); + return std::make_unique(l_copy); + }), + SEPINTER_SEPINTER_CONST_COLLECTION_T_REF, + "s"_a) + .def(py::init( [](const SepBase& s) { diff --git a/python/src/core/separators/codac2_py_SepUnion.cpp b/python/src/core/separators/codac2_py_SepUnion.cpp index c8d32c0f4..29ea6ff70 100644 --- a/python/src/core/separators/codac2_py_SepUnion.cpp +++ b/python/src/core/separators/codac2_py_SepUnion.cpp @@ -24,6 +24,17 @@ void export_SepUnion(py::module& m, py::class_& pysep) py::class_ exported(m, "SepUnion", pysep, SEPUNION_MAIN); exported + .def(py::init( + [](const py::list& l) + { + Collection l_copy; + for(const auto& li : l) + l_copy.push_back(li.cast().copy()); + return std::make_unique(l_copy); + }), + SEPUNION_SEPUNION_CONST_COLLECTION_T_REF, + "s"_a) + .def(py::init( [](const SepBase& s) { diff --git a/src/core/paver/codac2_pave.cpp b/src/core/paver/codac2_pave.cpp index 987cff977..c2d4cabad 100644 --- a/src/core/paver/codac2_pave.cpp +++ b/src/core/paver/codac2_pave.cpp @@ -28,7 +28,12 @@ namespace codac2 PavingOut pave(const IntervalVector& x0, const CtcBase& c, double eps, double& time, bool verbose) { - assert_release(eps > 0.); + return pave(x0,c,Vector::constant(x0.size(),eps),time,verbose); + } + + PavingOut pave(const IntervalVector& x0, const CtcBase& c, const Vector& veps, double& time, bool verbose) + { + assert_release(veps.min_coeff() > 0.); assert_release(!x0.is_empty()); clock_t t_start = clock(); @@ -53,9 +58,11 @@ namespace codac2 if(!get<0>(n->boxes()).is_empty()) { - if(get<0>(n->boxes()).max_diam() > eps) + IntervalVector w = get<0>(n->boxes()).array() / veps.array(); + + if(w.max_diam() > 1.) { - n->bisect(); + n->bisect([&w](const IntervalVector& x) { return x.bisect(w.max_diam_index()); }); l.push_back(n->left()); l.push_back(n->right()); } diff --git a/src/core/paver/codac2_pave.h b/src/core/paver/codac2_pave.h index d1b2781a1..242c6878e 100644 --- a/src/core/paver/codac2_pave.h +++ b/src/core/paver/codac2_pave.h @@ -23,6 +23,7 @@ namespace codac2 PavingOut pave(const IntervalVector& x0, std::shared_ptr> c, double eps, bool verbose = false); PavingOut pave(const IntervalVector& x0, const CtcBase& c, double eps, double& time, bool verbose = false); PavingOut pave(const IntervalVector& x0, const CtcBase& c, double eps, bool verbose = false); + PavingOut pave(const IntervalVector& x0, const CtcBase& c, const Vector& v_eps, double& time, bool verbose = false); PavingInOut pave(const IntervalVector& x0, std::shared_ptr s, double eps, bool verbose = false); PavingInOut pave(const IntervalVector& x0, const SepBase& s, double eps, bool verbose = false); diff --git a/src/core/separators/codac2_SepInter.h b/src/core/separators/codac2_SepInter.h index 979d2a753..23f95617b 100644 --- a/src/core/separators/codac2_SepInter.h +++ b/src/core/separators/codac2_SepInter.h @@ -20,6 +20,14 @@ namespace codac2 { public: + template + SepInter(const Collection& c) + : Sep([&c]() { + assert_release(!c.empty()); + return size_of(c.front()); + }()), _seps(c) + { } + template requires (IsSepBaseOrPtr && !std::is_same_v) SepInter(const S& s) diff --git a/src/core/separators/codac2_SepUnion.h b/src/core/separators/codac2_SepUnion.h index 33e5a87c8..e39461e07 100644 --- a/src/core/separators/codac2_SepUnion.h +++ b/src/core/separators/codac2_SepUnion.h @@ -20,6 +20,14 @@ namespace codac2 { public: + template + SepUnion(const Collection& c) + : Sep([&c]() { + assert_release(!c.empty()); + return size_of(c.front()); + }()), _seps(c) + { } + template requires (IsSepBaseOrPtr && !std::is_same_v) SepUnion(const S& s) diff --git a/src/core/trajectory/codac2_SampledTraj_operations.h b/src/core/trajectory/codac2_SampledTraj_operations.h index 4c6946348..8a02901d8 100644 --- a/src/core/trajectory/codac2_SampledTraj_operations.h +++ b/src/core/trajectory/codac2_SampledTraj_operations.h @@ -93,6 +93,12 @@ namespace codac2 inline Matrix operator_mul_vec(const Matrix& x1, const Vector& x2) { return x1 * x2; } + inline Vector operator_mul_aff(const Eigen::Affine2d& x1, const Vector& x2) + { + assert(x2.size() == 2); + return x1 * Eigen::Vector2d(x2); + } + template inline T operator_div(const X1& x1, const X2& x2) { return x1 / x2; } @@ -266,6 +272,14 @@ namespace codac2 inline SampledTraj operator*(const SampledTraj& x1, const SampledTraj& x2) macro_binary_traj_traj(operator_mul_vec); + /** \brief \f$x_1(\cdot)\cdot x_2\f$ + * \param x1 + * \param x2 + * \return trajectory output + */ + inline SampledTraj operator*(const Eigen::Affine2d& x1, const SampledTraj& x2) + macro_binary_real_traj(operator_mul_aff); + /** * \brief Operates *= * \param x1