Add formatting script, minor Bernstein additions

.clang-format

@@ -46,4 +46,3 @@ SpaceBeforeAssignmentOperators: true
 ContinuationIndentWidth: 4
 SortIncludes: false
 SpaceAfterCStyleCast: false
-BreakBeforeBraces: Linux

README.md

@@ -14,8 +14,13 @@ cd build
 cmake ..
 make
 ```
-If you're a developer, to make life easier, consider using your IDE of choice (VSCode recommended) with clangd as your language server. 
-Formatting can be fixed and standardised by clang-format using the included .clang-format file (with vscode, Ctrl-Shift-I with clangd applies it).
+# Contributing
+If you're a developer, to make life easier, consider using your IDE of choice (VSCode recommended) with clangd as your language server. Formatting can be fixed and standardised by 
+running the format script. You can also run clang-format on your files through your IDE (Ctrl-Shift-I on VSCode).
+```bash
+cd BeBOT
+./format.sh
+```
 
 # General Explanation
 The goal of this project is to put forward an algorithm to solve motion planning problems, enabling autonomous operations for cooperative vehicles navigating in complex environment in the presence of humans. The approach is to formulate the trajectory generation problem as an optimal control problem (OCP) and then use Bernstein polynomials to transcribe it into a nonlinear programming (NLP) problem which can be solved using off-the-shelf solvers. So, what does all of that mean?

format.sh

@@ -0,0 +1,3 @@
+#!/usr/bin/env bash
+SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
+find $SCRIPT_DIR/include/ $SCRIPT_DIR/src/ -iname *.hpp -o -iname *.cpp | xargs clang-format -i

include/bebot/algorithm.hpp

@@ -7,7 +7,6 @@
 #include <Eigen/Dense>
 #include <utility>
 
-
 // de Casteljau evaluation algorithm
 // 1D, 1 time point
 double deCasteljau_1d(const Eigen::VectorXd& control_points, double t);
@@ -21,4 +20,4 @@ deCasteljau_Nd(const Eigen::VectorXd& control_points, std::vector<double> t);
 
 // de Casteljau split algorithm
 std::pair<Eigen::MatrixXd, Eigen::MatrixXd>
-deCasteljau_split(const Eigen::VectorXd& control_points, double t);
+deCasteljau_split(const Eigen::VectorXd& control_points, double t);

include/bebot/bebot.hpp

@@ -6,10 +6,8 @@
 
 #include <Eigen/Dense>
 
-namespace bebot
-{
-class BeBOT
-{
+namespace bebot {
+class BeBOT {
   private:
     double _t0, _tf;
     Eigen::MatrixXd _cpts;

include/bebot/bernstein.hpp

@@ -3,14 +3,14 @@
 #include <utility>
 #include <vector>
 
-namespace bebot::bernstein
-{
-
-class Bernstein
-{
+namespace bebot::bernstein {
+class Bernstein {
   public:
     // Constructors
     Bernstein(Eigen::MatrixXd control_points, double initial_time = 0, double final_time = 1);
+    Bernstein(
+        std::vector<Eigen::Vector3d> control_points, double initial_time = 0, double final_time = 1
+    );
 
     // Information
     int dimension();
@@ -25,6 +25,7 @@ class Bernstein
     Bernstein operator-(Bernstein& b);
     Bernstein operator*(Bernstein& b);
     Bernstein operator/(Bernstein& b);
+    bool operator==(Bernstein& b);
 
     double mininum(int dimension, double tolerance = 1e-6);
 
@@ -39,7 +40,9 @@ class Bernstein
   private:
     Eigen::MatrixXd control_points_;
     double initial_time_;
+
     double final_time_;
+    double normalized_time(double t);
 };
 
 }  // namespace bebot::bernstein

src/common.cpp

@@ -3,14 +3,12 @@
 #include <numeric>
 
 // Python-like range functions
-std::vector<int> range(const int& start, const int& end)
-{
+std::vector<int> range(const int& start, const int& end) {
     std::vector<int> out(end - start);
     std::iota(out.begin(), out.end(), start);
     return out;
 }
 
-std::vector<int> range(const int& end)
-{
+std::vector<int> range(const int& end) {
     return range(0, end);
 }

src/polynomials/algorithms/algorithm.cpp

@@ -7,8 +7,7 @@
 // de Casteljau algorithm implementations
 // Might move this to /algorithms?
 // 1D, 1 time point
-double deCasteljau_1d(const Eigen::VectorXd& control_points, double t)
-{
+double deCasteljau_1d(const Eigen::VectorXd& control_points, double t) {
     Eigen::VectorXd control_points_new = control_points;
     while (control_points_new.size() > 1) {
         Eigen::VectorXd lower_order_points = Eigen::VectorXd::Zero(control_points_new.size() - 1);
@@ -21,8 +20,7 @@ double deCasteljau_1d(const Eigen::VectorXd& control_points, double t)
 }
 
 // 1D, T time points
-std::vector<double> deCasteljau_1d(const Eigen::VectorXd& control_points, std::vector<double> t)
-{
+std::vector<double> deCasteljau_1d(const Eigen::VectorXd& control_points, std::vector<double> t) {
     std::vector<double> out;
     std::transform(t.begin(), t.end(), std::back_inserter(out), [control_points](auto t) {
         return deCasteljau_1d(control_points, t);
@@ -31,8 +29,7 @@ std::vector<double> deCasteljau_1d(const Eigen::VectorXd& control_points, std::v
 }
 
 // N-D, 1 time point
-Eigen::VectorXd deCasteljau_Nd(const Eigen::MatrixXd& control_points, double t)
-{
+Eigen::VectorXd deCasteljau_Nd(const Eigen::MatrixXd& control_points, double t) {
     Eigen::VectorXd NdVec(control_points.rows());
     for (auto& row_num : range(control_points.rows())) {
         NdVec(row_num) = deCasteljau_1d(control_points.row(row_num), t);
@@ -42,8 +39,7 @@ Eigen::VectorXd deCasteljau_Nd(const Eigen::MatrixXd& control_points, double t)
 
 // N-D, T time points
 std::vector<Eigen::VectorXd>
-deCasteljau_Nd(const Eigen::MatrixXd& control_points, std::vector<double> t)
-{
+deCasteljau_Nd(const Eigen::MatrixXd& control_points, std::vector<double> t) {
     std::vector<Eigen::VectorXd> out;
     std::transform(t.begin(), t.end(), std::back_inserter(out), [control_points](auto t) {
         return deCasteljau_Nd(control_points, t);
@@ -52,8 +48,7 @@ deCasteljau_Nd(const Eigen::MatrixXd& control_points, std::vector<double> t)
 }
 
 std::pair<Eigen::MatrixXd, Eigen::MatrixXd>
-deCasteljau_split(const Eigen::MatrixXd& control_points, double t)
-{
+deCasteljau_split(const Eigen::MatrixXd& control_points, double t) {
     auto control_points_left = Eigen::MatrixXd(control_points.rows(), control_points.cols());
     auto control_points_right = Eigen::MatrixXd(control_points.rows(), control_points.cols());
     auto index = 0;

src/polynomials/primitives/bernstein.cpp

@@ -14,11 +14,9 @@
 using namespace bebot::bernstein;
 using namespace bebot::bernstein::internal;
 
-namespace
-{
+namespace {
 
-std::pair<Bernstein, Bernstein> temporally_align(Bernstein bernstein_1, Bernstein bernstein_2)
-{
+std::pair<Bernstein, Bernstein> temporally_align(Bernstein bernstein_1, Bernstein bernstein_2) {
     // TODO: implement
     // requires:
     //        split
@@ -28,31 +26,34 @@ std::pair<Bernstein, Bernstein> temporally_align(Bernstein bernstein_1, Bernstei
 }  // namespace
 
 Bernstein::Bernstein(Eigen::MatrixXd control_points, double initial_time, double final_time)
-  : control_points_(control_points), initial_time_(initial_time), final_time_(final_time)
-{
+  : control_points_(control_points), initial_time_(initial_time), final_time_(final_time) {
     if (final_time_ < initial_time_) {
         throw std::runtime_error("Final time must be more than initial time");
     }
 }
 
-int Bernstein::dimension()
-{
+Bernstein::Bernstein(
+    std::vector<Eigen::Vector3d> control_points, double initial_time, double final_time
+)
+  : Bernstein(
+        Eigen::Map<Eigen::MatrixXd>(control_points.front().data(), 3, control_points.size()),
+        initial_time, final_time
+    ) {
+}
+
+int Bernstein::dimension() {
     return control_points_.rows();
 }
 
-int Bernstein::degree()
-{
+int Bernstein::degree() {
     return control_points_.cols() - 1;
 }
 
-Eigen::VectorXd Bernstein::operator()(double t)
-{
-    auto tau = (t - initial_time_) / (final_time_ - initial_time_);
-    return deCasteljau_Nd(control_points_, tau);
+Eigen::VectorXd Bernstein::operator()(double t) {
+    return deCasteljau_Nd(control_points_, normalized_time(t));
 }
 
-Bernstein Bernstein::operator+(Bernstein& other)
-{
+Bernstein Bernstein::operator+(Bernstein& other) {
     if (initial_time_ == other.initial_time_ && final_time_ == other.final_time_) {
         return Bernstein(control_points_ + other.control_points_, initial_time_, final_time_);
     } else {
@@ -64,8 +65,7 @@ Bernstein Bernstein::operator+(Bernstein& other)
     }
 }
 
-Bernstein Bernstein::operator-(Bernstein& other)
-{
+Bernstein Bernstein::operator-(Bernstein& other) {
     if (initial_time_ == other.initial_time_ && final_time_ == other.final_time_) {
         return Bernstein(control_points_ - other.control_points_, initial_time_, final_time_);
     } else {
@@ -77,13 +77,21 @@ Bernstein Bernstein::operator-(Bernstein& other)
     }
 }
 
-Bernstein Bernstein::operator*(Bernstein& other)
-{
+Bernstein Bernstein::operator*(Bernstein& other) {
     if (other.dimension() != dimension()) {
         throw std::runtime_error("Dimensions must match");
     };
     Eigen::MatrixXd product_control_points(dimension(), degree() + other.degree() + 1);
     // TODO: implement
     Bernstein output{ product_control_points, initial_time_, final_time_ };
     return output;
-}
+}
+
+bool Bernstein::operator==(Bernstein& other) {
+    return control_points_.isApprox(other.control_points_) &&
+           initial_time_ == other.initial_time_ && final_time_ == other.final_time_;
+}
+
+double Bernstein::normalized_time(double t) {
+    return (t - initial_time_) / (final_time_ - initial_time_);
+}

src/polynomials/primitives/bernstein_helpers.cpp

@@ -3,18 +3,14 @@
 
 #include <numeric>
 
-namespace bebot::bernstein::internal
-{
-
-double binom(int n, int k)
-{
+namespace bebot::bernstein::internal {
+double binom(int n, int k) {
     // Stolen from cppreference :P
     // TODO: No idea how efficient this is
     return 1 / ((n + 1) * std::beta(n - k + 1, k + 1));
 }
 
-Eigen::MatrixXd bezier_coefficients(int n)
-{
+Eigen::MatrixXd bezier_coefficients(int n) {
     Eigen::MatrixXd coefficients = Eigen::MatrixXd::Zero(n + 1, n + 1);
     for (auto k : range(0, n + 1)) {
         for (auto i : range(k, n + 1)) {
@@ -25,8 +21,7 @@ Eigen::MatrixXd bezier_coefficients(int n)
 }
 
 // For computing norm
-Eigen::MatrixXd bezier_product_matrix(int n)
-{
+Eigen::MatrixXd bezier_product_matrix(int n) {
     Eigen::MatrixXd prod_matrix(2 * n + 1, (n + 1) * (n + 1));
     for (auto j : range(2 * n + 1)) {
         auto denominator = binom(2 * n, j);

src/polynomials/primitives/bernstein_helpers.hpp

@@ -1,9 +1,10 @@
+#pragma once
+
 #include <Eigen/Dense>
 
 #include <vector>
 
-namespace bebot::bernstein::internal
-{
+namespace bebot::bernstein::internal {
 
 double binom(int n, int k);