trajopt_util.hpp

// Copyright (c) TrajoptLib contributors
 
#pragma once
 
#include <cmath>
#include <cstddef>
#include <numbers>
#include <numeric>
#include <vector>
 
namespace trajopt {

inline size_t get_index(const std::vector<size_t>& N, size_t wpt_index,
                        size_t sample_index = 0) {
  return std::accumulate(N.begin(), N.begin() + wpt_index, size_t{0}) +
         sample_index;
}

inline std::vector<double> linspace(double start, double end,
                                    size_t num_samples) {
  std::vector<double> result;
  double delta = (end - start) / num_samples;
  for (size_t i = 1; i <= num_samples; ++i) {
    result.push_back(start + i * delta);
  }
  return result;
}

constexpr double input_modulus(double input, double minimum_input,
                               double maximum_input) {
  double modulus = maximum_input - minimum_input;
 
  // Wrap input if it's above the maximum input
  int numMax = (input - minimum_input) / modulus;
  input -= numMax * modulus;
 
  // Wrap input if it's below the minimum input
  int numMin = (input - maximum_input) / modulus;
  input -= numMin * modulus;
 
  return input;
}

constexpr double angle_modulus(double angle) {
  return input_modulus(angle, -std::numbers::pi, std::numbers::pi);
}

inline std::vector<double> angle_linspace(double start, double end,
                                          size_t num_samples) {
  return linspace(start, start + angle_modulus(end - start), num_samples);
}

inline double calculate_trapezoidal_time(double distance, double velocity,
                                         double acceleration) {
  if (distance > ((velocity * velocity) / acceleration)) {
    // Velocity profile is shaped like a trapezoid
    return distance / velocity + velocity / acceleration;
  } else {
    // Velocity profile is shaped like a triangle
    return 2.0 * std::sqrt(distance * acceleration) / acceleration;
  }
}
 
}  // namespace trajopt