[graphics] colormaps for trajectories

codac-team · Dec 18, 2024 · d88420b · d88420b
1 parent 9882f6d
commit d88420b
Show file tree

Hide file tree

Showing 3 changed files with 97 additions and 0 deletions.
diff --git a/python/src/graphics/figures/codac2_py_Figure2D.cpp b/python/src/graphics/figures/codac2_py_Figure2D.cpp
@@ -151,6 +151,31 @@ void export_Figure2D(py::module& m)
       VOID_FIGURE2D_DRAW_TRAJECTORY_CONST_ANALYTICTRAJECTORY_VECTOROPVALUE_REF_CONST_STYLEPROPERTIES_REF,
       "x"_a, "s"_a=StyleProperties())
 
+    .def("draw_trajectory", [](Figure2D& fig, py::object x, const ColorMap& cmap)
+        {
+          py::object x_traj = x.attr("traj");
+
+          if(x_traj)
+          {
+            if(py::isinstance<AnalyticTrajectory<VectorOpValue>>(x_traj))
+            {
+              fig.draw_trajectory(x_traj.cast<AnalyticTrajectory<VectorOpValue>>(),cmap);
+              return;
+            }
+
+            else if(py::isinstance<SampledTrajectory<Vector>>(x_traj))
+            {
+              fig.draw_trajectory(x_traj.cast<SampledTrajectory<Vector>>(),cmap);
+              return;
+            }
+          }
+
+          assert_release(false &&
+            "provided trajectory is not of type AnalyticTrajectory<VectorOpValue> or SampledTrajectory<Vector>");
+        },
+      VOID_FIGURE2D_DRAW_TRAJECTORY_CONST_ANALYTICTRAJECTORY_VECTOROPVALUE_REF_CONST_STYLEPROPERTIES_REF,
+      "x"_a, "cmap"_a)
+
     // Robots
 
     .def("draw_tank", &Figure2D::draw_tank,
@@ -247,6 +272,31 @@ void export_Figure2D(py::module& m)
       VOID_FIGURE2D_DRAW_TRAJECTORY_CONST_ANALYTICTRAJECTORY_VECTOROPVALUE_REF_CONST_STYLEPROPERTIES_REF,
       "x"_a, "s"_a=StyleProperties())
 
+    .def_static("draw_trajectory", [](py::object x, const ColorMap& cmap)
+        {
+          py::object x_traj = x.attr("traj");
+
+          if(x_traj)
+          {
+            if(py::isinstance<AnalyticTrajectory<VectorOpValue>>(x_traj))
+            {
+              DefaultView::draw_trajectory(x_traj.cast<AnalyticTrajectory<VectorOpValue>>(),cmap);
+              return;
+            }
+
+            else if(py::isinstance<SampledTrajectory<Vector>>(x_traj))
+            {
+              DefaultView::draw_trajectory(x_traj.cast<SampledTrajectory<Vector>>(),cmap);
+              return;
+            }
+          }
+
+          assert_release(false &&
+            "provided trajectory is not of type AnalyticTrajectory<VectorOpValue> or SampledTrajectory<Vector>");
+        },
+      VOID_FIGURE2D_DRAW_TRAJECTORY_CONST_ANALYTICTRAJECTORY_VECTOROPVALUE_REF_CONST_STYLEPROPERTIES_REF,
+      "x"_a, "cmap"_a)
+
     // Robots
 
     .def_static("draw_tank", &DefaultView::draw_tank,

diff --git a/src/graphics/figures/codac2_Figure2D.cpp b/src/graphics/figures/codac2_Figure2D.cpp
@@ -138,6 +138,16 @@ void Figure2D::draw_ring(const Vector& c, const Interval& r, const StyleProperti
       output_fig->draw_ring(c,r,s);
 }
 
+void Figure2D::draw_line(const Vector& x, const Vector& y, const StyleProperties& s)
+{
+  assert_release(x.size() == y.size());
+  assert_release(x.size() == 2);
+  vector<Vector> points = {x,y};
+
+  for(const auto& output_fig : _output_figures)
+    output_fig->draw_polyline(points,1e-3*scaled_unit(),s);
+}
+
 void Figure2D::draw_polyline(const vector<Vector>& x, const StyleProperties& s)
 {
   draw_polyline(x, 1e-3*scaled_unit(), s);
@@ -212,6 +222,26 @@ void Figure2D::draw_trajectory(const AnalyticTrajectory<VectorOpValue>& x, const
   draw_trajectory(x.sampled(x.tdomain().diam()/1e4), s);
 }
 
+void Figure2D::draw_trajectory(const SampledTrajectory<Vector>& x, const ColorMap& cmap)
+{
+  assert_release(this->size() <= x.size());
+
+  std::vector<Vector> values(2);
+  values[0] = x.begin()->second;
+  for (auto it = std::next(x.begin()); it != x.end(); ++it)
+  {
+    values[1] = it->second;
+    Color color = cmap.color((it->first - x.begin()->first) / (x.rbegin()->first - x.begin()->first));
+    draw_polyline(values, color);
+    values[0] = values[1];
+  }
+}
+
+void Figure2D::draw_trajectory(const AnalyticTrajectory<VectorOpValue>& x, const ColorMap& cmap)
+{
+  draw_trajectory(x.sampled(x.tdomain().diam()/1e4), cmap);
+}
+
 void Figure2D::draw_tank(const Vector& x, float size, const StyleProperties& s)
 {
   assert_release(this->size() <= x.size()+1);

diff --git a/src/graphics/figures/codac2_Figure2D.h b/src/graphics/figures/codac2_Figure2D.h
@@ -16,6 +16,7 @@
 #include "codac2_Figure2DInterface.h"
 #include "codac2_OutputFigure2D.h"
 #include "codac2_Paving.h"
+#include "codac2_ColorMap.h"
 
 #define DEFAULT_FIG_NAME "Codac - default view"
 
@@ -85,13 +86,17 @@ namespace codac2
       void draw_box(const IntervalVector& x, const StyleProperties& s = StyleProperties());
       void draw_circle(const Vector& c, double r, const StyleProperties& s = StyleProperties());
       void draw_ring(const Vector& c, const Interval& r, const StyleProperties& s = StyleProperties());
+      void draw_line(const Vector& x1, const Vector& x2, const StyleProperties& s = StyleProperties());
       void draw_polyline(const std::vector<Vector>& x, const StyleProperties& s = StyleProperties());
       void draw_polyline(const std::vector<Vector>& x, float tip_length, const StyleProperties& s = StyleProperties());
       void draw_polygone(const std::vector<Vector>& x, const StyleProperties& s = StyleProperties());
       void draw_pie(const Vector& c, const Interval& r, const Interval& theta, const StyleProperties& s = StyleProperties());
       void draw_ellipse(const Vector& c, const Vector& ab, double theta, const StyleProperties& s = StyleProperties());
       void draw_trajectory(const SampledTrajectory<Vector>& x, const StyleProperties& s = StyleProperties());
       void draw_trajectory(const AnalyticTrajectory<VectorOpValue>& x, const StyleProperties& s = StyleProperties());
+      void draw_trajectory(const SampledTrajectory<Vector>& x, const ColorMap& cmap);
+      void draw_trajectory(const AnalyticTrajectory<VectorOpValue>& x, const ColorMap& cmap);
+
       // Robots
       void draw_tank(const Vector& x, float size, const StyleProperties& s = StyleProperties());
       void draw_AUV(const Vector& x, float size, const StyleProperties& s = StyleProperties());
@@ -218,6 +223,18 @@ namespace codac2
         selected_fig()->draw_trajectory(x,s);
       }
 
+      static void draw_trajectory(const SampledTrajectory<Vector>& x, const ColorMap& cmap)
+      {
+        auto_init();
+        selected_fig()->draw_trajectory(x,cmap);
+      }
+
+      static void draw_trajectory(const AnalyticTrajectory<VectorOpValue>& x, const ColorMap& cmap)
+      {
+        auto_init();
+        selected_fig()->draw_trajectory(x,cmap);
+      }
+
       // Robots
 
       static void draw_tank(const Vector& x, float size, const StyleProperties& s = StyleProperties())