Adding directions_2d and directions_3d

CMakeLists.txt

@@ -51,6 +51,7 @@ add_library(libplaco SHARED
     src/placo/tools/prioritized.cpp
     src/placo/tools/cubic_spline.cpp
     src/placo/tools/cubic_spline_3d.cpp
+    src/placo/tools/directions.cpp
 
     # Problem formulation
     src/placo/problem/problem.cpp

bindings/expose-tools.cpp

@@ -9,6 +9,7 @@
 #include "placo/tools/cubic_spline_3d.h"
 #include "placo/tools/axises_mask.h"
 #include "placo/tools/prioritized.h"
+#include "placo/tools/directions.h"
 #include "expose-utils.hpp"
 #ifdef HAVE_RHOBAN_UTILS
 #include "rhoban_utils/history/history.h"
@@ -24,6 +25,7 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(loadReplays_overloads, loadReplays, 1, 2)
 
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(set_axises_overloads, set_axises, 1, 2);
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(configure_overloads, configure, 2, 3);
+BOOST_PYTHON_FUNCTION_OVERLOADS(directions_3d_overloads, directions_3d, 1, 2);
 
 void exposeTools()
 {
@@ -33,6 +35,8 @@ void exposeTools()
   def("frame_yaw", &frame_yaw);
   def("flatten_on_floor", &flatten_on_floor);
   def("optimal_transformation", &optimal_transformation);
+  def("directions_2d", &directions_2d);
+  def("directions_3d", &directions_3d, directions_3d_overloads());
 
   exposeStdVector<int>("vector_int");
   exposeStdVector<double>("vector_double");

src/placo/tools/directions.cpp

@@ -0,0 +1,42 @@
+#include "directions.h"
+
+namespace placo::tools
+{
+Eigen::MatrixXd directions_2d(int n)
+{
+  Eigen::MatrixXd directions(n, 2);
+
+  for (int i = 0; i < n; i++)
+  {
+    double angle = 2 * M_PI * i / n;
+    directions(i, 0) = cos(angle);
+    directions(i, 1) = sin(angle);
+  }
+
+  return directions;
+}
+
+Eigen::MatrixXd directions_3d(int n, double epsilon)
+{
+  Eigen::MatrixXd directions(n, 3);
+
+  // Golden ratio
+  double phi = (1 + sqrt(5)) / 2;
+
+  for (int i = 0; i < n; i++)
+  {
+    double x = fmod(i / phi, 1);
+    double y = (i + epsilon) / (n - 1 + 2 * epsilon);
+
+    double alpha = 2 * M_PI * x;
+    double beta = acos(1 - 2 * y);
+
+    directions(i, 0) = sin(beta) * cos(alpha);
+    directions(i, 1) = sin(beta) * sin(alpha);
+    directions(i, 2) = cos(beta);
+  }
+
+  return directions;
+}
+
+}  // namespace placo::tools

src/placo/tools/directions.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <vector>
+#include <Eigen/Dense>
+
+namespace placo::tools
+{
+/**
+ * @brief Generates a set of directions in 2D
+ * @param n the number of directions
+ * @return matrix of size n x 2
+ */
+Eigen::MatrixXd directions_2d(int n);
+
+/**
+ * @brief Generates a set of directions in 3D, using Fibonacci lattice
+ * @param n the number of directions
+ * @param epsilon the epsilon parameter for the Fibonacci lattice
+ * @return matrix of size n x 3
+ */
+Eigen::MatrixXd directions_3d(int n, double epsilon = 0.5);
+}  // namespace placo::tools