File content as of revision 2:e4b06a9b3ff2:

#include <array>
#include <cmath>
#include <algorithm>

template <size_t wheelSize>
class Omni {
  public:
    Omni(const std::array<float, wheelSize>& radians) : radians(radians) {}
    
    void computeXY(const float& x, const float& y, const float& a) {
      computeCircular(std::hypot(x, y), std::atan2(x, y), a);
    }
    
    void computeCircular(const float& r, const float& rad, const float& a) {
      for(size_t i = 0; i < wheelSize; i++) {
        m_outs[i] = sin(rad + radians[i]) * r + a;
      }
      const float maxOut = *std::max_element(m_outs.begin(), m_outs.end(), [](const auto& a, const auto& b){return std::abs(a) < std::abs(b);});
      if(maxOut > 1.0) {
        for(auto& out: m_outs) {
          out *= (1.0 / maxOut);
        }  
      }
    }
    
    const std::array<float, wheelSize> outs() { return m_outs; }
    
    typename std::array<float, wheelSize>::iterator begin() { return m_outs.begin(); }
    typename std::array<float, wheelSize>::iterator end() { return m_outs.end(); }
    
  private:
    std::array<float, wheelSize> radians;
    std::array<float, wheelSize> m_outs;
};