[tuto] added Wall class + CtcUnion binding

python/src/core/contractors/static/codac_py_Ctc.cpp

@@ -49,8 +49,11 @@ py::class_<Ctc,pyCtc> export_Ctc(py::module& m)
 
     .def("__or__", [](Ctc& c1, Ctc& c2)
       {
-        return new CtcUnion(c1, c2);
+        CtcUnion *cu = new CtcUnion(2);
+        cu->add_raw_ptr(&c1);
+        cu->add_raw_ptr(&c2);
         // todo: manage delete
+        return cu;
       },
       DOC_CTC_OR,
       py::return_value_policy::take_ownership,
@@ -77,9 +80,34 @@ py::class_<Ctc,pyCtc> export_Ctc(py::module& m)
   ;
 
   // Export CtcUnion
-  py::class_<CtcUnion>(m, "CtcUnion", ctc, DOC_CTCUNION_TYPE)
-    .def(py::init<ibex::Array<Ctc>>(), py::keep_alive<1,2>(), "list"_a)
+  py::class_<CtcUnion>(m, "CtcUnion", ctc, "todo")
+    .def(py::init<int>(), "nb_var"_a)
+    .def(py::init([](Ctc& c1)
+      {
+        CtcUnion *cu = new CtcUnion(c1.nb_var);
+        cu->add_raw_ptr(&c1);
+        return cu;
+      }),
+      py::keep_alive<0,1>(), "c1"_a)
+    .def(py::init([](Ctc& c1, Ctc& c2)
+      {
+        CtcUnion *cu = new CtcUnion(c1.nb_var);
+        cu->add_raw_ptr(&c1);
+        cu->add_raw_ptr(&c2);
+        return cu;
+      }),
+      py::keep_alive<0,1>(), py::keep_alive<0,2>(), "c1"_a, "c2"_a)
+    .def(py::init([](Ctc& c1, Ctc& c2, Ctc& c3)
+      {
+        CtcUnion *cu = new CtcUnion(c1.nb_var);
+        cu->add_raw_ptr(&c1);
+        cu->add_raw_ptr(&c2);
+        cu->add_raw_ptr(&c3);
+        return cu;
+      }),
+      py::keep_alive<0,1>(), py::keep_alive<0,2>(), py::keep_alive<0,3>(), "c1"_a, "c2"_a, "c3"_a)
     .def("contract", (void (Ctc::*)(IntervalVector&)) &CtcUnion::contract)
+    .def("__ior__", [](CtcUnion& cu, Ctc& c) { return cu.add_raw_ptr(&c); }, py::keep_alive<1,2>(), py::return_value_policy::take_ownership)
     ;
 
   // Export CtcCompo

python/src/core/graphics/codac_py_VIBesFig.cpp

@@ -92,5 +92,9 @@ void export_VIBesFig(py::module& m)
     .def("draw_vehicle", (void (VIBesFig::*)(double,double,double,double,const string&,const vibes::Params &))&VIBesFig::draw_vehicle,
       VIBESFIG_VOID_DRAW_VEHICLE_DOUBLE_DOUBLE_DOUBLE_DOUBLE_STRING_VIBESPARAMS,
       "x"_a, "y"_a, "heading"_a, "size"_a, "color"_a="", "params"_a=vibes::Params())
+
+    .def("draw_line", (void (VIBesFig::*)(const std::vector<double>&,const std::vector<double>&,const string&,const vibes::Params &))&VIBesFig::draw_line,
+      "todo",
+      "v_x"_a, "v_y"_a, "color"_a="", "params"_a=vibes::Params())
   ;
 }

python/src/robotics/codac_py_Wall.cpp

@@ -0,0 +1,42 @@
+/** 
+ *  \file
+ *  Wall Python binding
+ * ----------------------------------------------------------------------------
+ *  \date       2024
+ *  \author     Simon Rohou, Benoît Desrochers
+ *  \copyright  Copyright 2024 Codac Team
+ *  \license    This program is distributed under the terms of
+ *              the GNU Lesser General Public License (LGPL).
+ */
+
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <pybind11/operators.h>
+#include <pybind11/functional.h>
+#include "codac_type_caster.h"
+
+#include "codac_Wall.h"
+// Generated file from Doxygen XML (doxygen2docstring.py):
+#include "codac_py_Wall_docs.h"
+
+using namespace std;
+using namespace codac;
+namespace py = pybind11;
+using namespace pybind11::literals;
+
+
+void export_Wall(py::module& m)
+{
+  py::class_<Wall> wall(m, "Wall", WALL_MAIN);
+  wall
+
+    .def(py::init<const Vector&,const Vector&>(), "todo")
+    .def("contains", &Wall::contains, "todo", "p"_a)
+    .def(py::self & py::self)
+    .def_readwrite("c1", &Wall::c1)
+    .def_readwrite("c2", &Wall::c2)
+  ;
+
+  m.def("shorter_dist_to_walls", &shorter_dist_to_walls, "todo", "v_walls"_a, "p"_a, "bearing"_a);
+  m.def("shorter_contact_to_walls", &shorter_contact_to_walls, "todo", "v_walls"_a, "p"_a);
+}

src/core/contractors/static/codac_CtcSegment.cpp

@@ -14,7 +14,8 @@ using namespace std;
 namespace codac {
 
 CtcSegment::CtcSegment(double ax, double ay, double bx, double by) : Ctc(2),
-    X_with_params(2+4) {
+    X_with_params(2+4)
+{
 
     init();
 
@@ -24,6 +25,11 @@ CtcSegment::CtcSegment(double ax, double ay, double bx, double by) : Ctc(2),
     X_with_params[5] = Interval(by);
 }
 
+CtcSegment::CtcSegment(const CtcSegment& ctc) : CtcSegment(ctc.X_with_params[2].lb(),ctc.X_with_params[3].lb(),ctc.X_with_params[4].lb(),ctc.X_with_params[5].lb())
+{
+
+}
+
 CtcSegment::CtcSegment() : Ctc(6), X_with_params(0 /* unused */) {
   init();
 }

src/core/contractors/static/codac_CtcSegment.h

@@ -15,15 +15,15 @@
 #include "ibex_CtcFwdBwd.h"
 
 
+namespace codac {
+
+
 using ibex::Interval;
 using ibex::IntervalVector;
 using ibex::Ctc;
 using ibex::NumConstraint;
 using ibex::CtcFwdBwd;
 
-
-namespace codac {
-
 /**
  * \ingroup geometry
  *
@@ -58,6 +58,8 @@ class CtcSegment : public Ctc {
    */
   CtcSegment();
 
+  CtcSegment(const CtcSegment& ctc);
+
   /**
    * \brief Contract a box.
    * \param box to be contracted

src/core/contractors/static/codac_CtcUnion.h

@@ -2,21 +2,82 @@
  *  \file
  *  CtcUnion class
  * ----------------------------------------------------------------------------
- *  \date       2022
+ *  \date       2024
  *  \author     Simon Rohou
- *  \copyright  Copyright 2022 Codac Team
+ *  \copyright  Copyright 2024 Codac Team
  *  \license    This program is distributed under the terms of
  *              the GNU Lesser General Public License (LGPL).
  */
 
 #ifndef __CODAC_CTCUNION_H__
 #define __CODAC_CTCUNION_H__
 
-#include "ibex_CtcUnion.h"
+#include <memory>
+//#include "ibex_CtcUnion.h"
+#include "codac_Ctc.h"
 
 namespace codac
 {
-  using ibex::CtcUnion;
+  class CtcUnion : public Ctc
+  {
+    public:
+
+      CtcUnion(int nb_var) : Ctc(nb_var)
+      { }
+
+      template<typename C1>
+      CtcUnion(const C1& c1) : Ctc(c1.nb_var)
+      {
+        _v_ctc.push_back(std::make_shared<C1>(c1));
+      }
+
+      template<typename C1,typename... C>
+      CtcUnion(const C1& c1, const C&... c) : CtcUnion(c1)
+      {
+        (_v_ctc.push_back(std::make_shared<C>(c)), ...);
+        for(const auto& ci : _v_ctc) { assert(ci->nb_var == nb_var); }
+      }
+
+      void contract(IntervalVector& x)
+      {
+        IntervalVector result(nb_var, Interval::empty_set());
+
+        for(auto& ci : _v_ctc)
+        {
+          IntervalVector saved_x = x;
+          ci->contract(saved_x);
+          result |= saved_x;
+        }
+
+        for(auto& ci : _v_ctc_ptrs)
+        {
+          IntervalVector saved_x = x;
+          ci->contract(saved_x);
+          result |= saved_x;
+        }
+
+        x = result;
+      }
+
+      template<typename C>
+      CtcUnion& operator|=(const C& c)
+      {
+        assert(c.nb_var == nb_var);
+        _v_ctc.push_back(std::make_shared<C>(c));
+        return *this;
+      }
+
+      CtcUnion& add_raw_ptr(Ctc *c)
+      {
+        _v_ctc_ptrs.push_back(c);
+        return *this;
+      }
+
+    protected:
+
+      std::vector<std::shared_ptr<Ctc>> _v_ctc;
+      std::vector<Ctc*> _v_ctc_ptrs;
+  };
 }
 
 #endif

src/robotics/CMakeLists.txt

@@ -6,6 +6,8 @@
                   ${CMAKE_CURRENT_SOURCE_DIR}/graphics/codac_VIBesFigMap.h
                   ${CMAKE_CURRENT_SOURCE_DIR}/objects/codac_Beacon.cpp
                   ${CMAKE_CURRENT_SOURCE_DIR}/objects/codac_Beacon.h
+                  ${CMAKE_CURRENT_SOURCE_DIR}/objects/codac_Wall.cpp
+                  ${CMAKE_CURRENT_SOURCE_DIR}/objects/codac_Wall.h
                   ${CMAKE_CURRENT_SOURCE_DIR}/loops/codac_TPlane.h
                   ${CMAKE_CURRENT_SOURCE_DIR}/loops/codac_TPlane.cpp
                   ${CMAKE_CURRENT_SOURCE_DIR}/data/codac_DataLoaderLissajous.cpp

src/robotics/objects/codac_Wall.cpp

@@ -0,0 +1,82 @@
+/** 
+ *  Wall class
+ * ----------------------------------------------------------------------------
+ *  \date       2024
+ *  \author     Simon Rohou
+ *  \copyright  Copyright 2024 Codac Team
+ *  \license    This program is distributed under the terms of
+ *              the GNU Lesser General Public License (LGPL).
+ */
+
+#include <iostream>
+#include "codac_Wall.h"
+#include "codac_predef_values.h"
+
+using namespace std;
+using namespace ibex;
+
+namespace codac
+{
+  Wall::Wall(const Vector& c1_, const Vector& c2_) : c1(c1_), c2(c2_)
+  {
+  }
+
+  bool Wall::contains(const Vector& p) const
+  {
+    Vector ab { c2[0]-c1[0], c2[1]-c1[1] };
+    Vector ac { p[0]-c1[0], p[1]-c1[1] };
+
+    double dp_AB = ab[0]*ab[0] + ab[1]*ab[1];
+    double dp_AC = ab[0]*ac[0] + ab[1]*ac[1];
+    return Interval(0.,dp_AB).contains(dp_AC);
+  }
+
+  Vector operator&(const Wall& w1, const Wall& w2)
+  {
+    const double& x1 = w1.c1[0]; const double& x2 = w1.c2[0];
+    const double& x3 = w2.c1[0]; const double& x4 = w2.c2[0];
+
+    const double& y1 = w1.c1[1]; const double& y2 = w1.c2[1];
+    const double& y3 = w2.c1[1]; const double& y4 = w2.c2[1];
+
+    return {
+      ((x1*y2-y1*x2)*(x3-x4)-(x1-x2)*(x3*y4-y3*x4))/((x1-x2)*(y3-y4)-(y1-y2)*(x3-x4)),
+      ((x1*y2-y1*x2)*(y3-y4)-(y1-y2)*(x3*y4-y3*x4))/((x1-x2)*(y3-y4)-(y1-y2)*(x3-x4))
+    };
+  }
+
+  double shorter_dist_to_walls(const std::vector<Wall>& v_walls, const Vector& p, double bearing)
+  {
+    Wall w0 { p, { p[0]+999999.*cos(bearing), p[1]+999999.*sin(bearing) }};
+    double min_dist = oo;
+
+    for(const auto& wi : v_walls)
+    {
+      Vector pi = w0 & wi;
+      if(!wi.contains(pi) || !w0.contains(pi))
+        continue;
+      double dist = pow(p[0]-pi[0],2) + pow(p[1]-pi[1],2);
+      min_dist = dist < min_dist ? dist : min_dist;
+    }
+
+    return sqrt(min_dist);
+  }
+
+  Vector shorter_contact_to_walls(const std::vector<Wall>& v_walls, const Vector& p)
+  {
+    double min_dist = oo;
+    double bearing;
+
+    for(double a = 0. ; a < 2.*M_PI ; a+=0.1)
+    {
+      double dist = shorter_dist_to_walls(v_walls, p, a);
+      if(dist < min_dist)
+      {
+        min_dist = dist;
+        bearing = a;
+      }
+    }
+
+    return { min_dist, bearing };
+  }
+}

src/robotics/objects/codac_Wall.h

@@ -0,0 +1,37 @@
+/** 
+ *  Beacon class
+ * ----------------------------------------------------------------------------
+ *  \date       2024
+ *  \author     Simon Rohou
+ *  \copyright  Copyright 2024 Codac Team
+ *  \license    This program is distributed under the terms of
+ *              the GNU Lesser General Public License (LGPL).
+ */
+
+#ifndef __CODAC_WALL_H__
+#define __CODAC_WALL_H__
+
+#include "codac_Vector.h"
+#include "codac_IntervalVector.h"
+
+namespace codac
+{
+  class Wall
+  {
+    public:
+
+      Wall(const Vector& c1, const Vector& c2);
+      bool contains(const Vector& p) const;
+
+    //protected:
+
+      Vector c1, c2;
+  };
+
+  Vector operator&(const Wall& w1, const Wall& w2);
+
+  double shorter_dist_to_walls(const std::vector<Wall>& v_walls, const Vector& p, double bearing);
+  Vector shorter_contact_to_walls(const std::vector<Wall>& v_walls, const Vector& p);
+}
+
+#endif