File content as of revision 0:14717ae9875d:

#include <array>
#include <iterator>
#include <cmath>

template <size_t wheelSize>
class Omni {
  public:
    Omni(const std::array<float, wheelSize>& radians) : radians(radians) {
        for(auto& out: m_outs) {
            out = 0.0;
        }
    }
    
    void computeXY(float x, float y, float a) {
      computeCircular(hypot(x, y), atan2(x, y), a);
    }
    
    void computeCircular(float r, float rad, float a) {
      for(int i = 0; i < wheelSize; i++) {
        m_outs[i] = sin(rad + radians[i]) * r + a;
      }
      const float maxOut = std::max(std::abs(*std::max(m_outs.begin(), m_outs.end())), std::abs(*std::min(m_outs.begin(), m_outs.end())));
      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;
};