Merge pull request #388 from SimonRohou/codac2_dev

[sep] SepInter and SepUnion can be built from a list of separators

Author: SimonRohou

doc/manual/manual/contractors/shape/index.rst

@@ -1,5 +1,5 @@
-Geometric contractors
-=====================
+Shape contractors
+=================
 
 .. toctree::
 

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<Vector> 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

python/src/core/separators/codac2_py_SepInter.cpp

@@ -24,6 +24,17 @@ void export_SepInter(py::module& m, py::class_<SepBase,pySep>& pysep)
   py::class_<SepInter> exported(m, "SepInter", pysep, SEPINTER_MAIN);
   exported
 
+    .def(py::init(
+        [](const py::list& l)
+        {
+          Collection<SepBase> l_copy;
+          for(const auto& li : l)
+            l_copy.push_back(li.cast<SepBase&>().copy());
+          return std::make_unique<SepInter>(l_copy);
+        }),
+      SEPINTER_SEPINTER_CONST_COLLECTION_T_REF,
+      "s"_a)
+
     .def(py::init(
         [](const SepBase& s)
         {

python/src/core/separators/codac2_py_SepUnion.cpp

@@ -24,6 +24,17 @@ void export_SepUnion(py::module& m, py::class_<SepBase,pySep>& pysep)
   py::class_<SepUnion> exported(m, "SepUnion", pysep, SEPUNION_MAIN);
   exported
 
+    .def(py::init(
+        [](const py::list& l)
+        {
+          Collection<SepBase> l_copy;
+          for(const auto& li : l)
+            l_copy.push_back(li.cast<SepBase&>().copy());
+          return std::make_unique<SepUnion>(l_copy);
+        }),
+      SEPUNION_SEPUNION_CONST_COLLECTION_T_REF,
+      "s"_a)
+
     .def(py::init(
         [](const SepBase& s)
         {

src/core/paver/codac2_pave.cpp

@@ -28,7 +28,12 @@ namespace codac2
 
   PavingOut pave(const IntervalVector& x0, const CtcBase<IntervalVector>& 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<IntervalVector>& 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());
         }

src/core/paver/codac2_pave.h

@@ -23,6 +23,7 @@ namespace codac2
   PavingOut pave(const IntervalVector& x0, std::shared_ptr<const CtcBase<IntervalVector>> c, double eps, bool verbose = false);
   PavingOut pave(const IntervalVector& x0, const CtcBase<IntervalVector>& c, double eps, double& time, bool verbose = false);
   PavingOut pave(const IntervalVector& x0, const CtcBase<IntervalVector>& c, double eps, bool verbose = false);
+  PavingOut pave(const IntervalVector& x0, const CtcBase<IntervalVector>& c, const Vector& v_eps, double& time, bool verbose = false);
 
   PavingInOut pave(const IntervalVector& x0, std::shared_ptr<const SepBase> s, double eps, bool verbose = false);
   PavingInOut pave(const IntervalVector& x0, const SepBase& s, double eps, bool verbose = false);

src/core/separators/codac2_SepInter.h

@@ -20,6 +20,14 @@ namespace codac2
   {
     public:
 
+      template<typename T>
+      SepInter(const Collection<T>& c)
+        : Sep<SepInter>([&c]() {
+          assert_release(!c.empty());
+            return size_of(c.front());
+          }()), _seps(c)
+      { }
+
       template<typename S>
         requires (IsSepBaseOrPtr<S> && !std::is_same_v<SepInter,S>)
       SepInter(const S& s)

src/core/separators/codac2_SepUnion.h

@@ -20,6 +20,14 @@ namespace codac2
   {
     public:
 
+      template<typename T>
+      SepUnion(const Collection<T>& c)
+        : Sep<SepUnion>([&c]() {
+          assert_release(!c.empty());
+            return size_of(c.front());
+          }()), _seps(c)
+      { }
+
       template<typename S>
         requires (IsSepBaseOrPtr<S> && !std::is_same_v<SepUnion,S>)
       SepUnion(const S& s)

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<typename T,typename X1,typename X2>
   inline T operator_div(const X1& x1, const X2& x2) { return x1 / x2; }
 
@@ -266,6 +272,14 @@ namespace codac2
   inline SampledTraj<Vector> operator*(const SampledTraj<Matrix>& x1, const SampledTraj<Vector>& 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<Vector> operator*(const Eigen::Affine2d& x1, const SampledTraj<Vector>& x2)
+    macro_binary_real_traj(operator_mul_aff);
+
   /**
    * \brief Operates *=
    * \param x1