File content as of revision 5:f2fc4a0b6a6f:

#include "omni.h"

inline double fmax(double a, double b)
{
    if(a > b) {
        return a;
    } else {
        return b;
    }
}

inline double fmin(double a, double b)
{
    if(a < b) {
        return a;
    } else {
        return b;
    }
}

inline double toRadians(double degrees) {
    return degrees * (M_PI / 180.0);
}

Omni::Omni(int wheels, double initDegree) :
    wheels(wheels), initDegree(initDegree)
{
    for(int i = 0; i < wheels; i++) {
        wheel[i] = 0;
    }
}

bool Omni::computeXY(double parallelVector[], double moment)
{
    if(wheels < 3 || wheels > 4) {
        return 0;
    }

    double polar[2];

    polar[0] = atan2(parallelVector[1], parallelVector[0]);
    polar[1] = hypot(parallelVector[0], parallelVector[1]);

    return computePolar(polar, moment);
}

bool Omni::computePolar(double parallelVector[], double moment)
{
    if(wheels < 3 || wheels > 4) {
        return 0;
    }

    if(parallelVector[1] < 0 || parallelVector[1] > 1.0) {
        return 0;
    }

    double parallel[4] = {0};
    double parallelMax = 0;
    double parallelMin = 0;

    for(int i = 0; i < wheels; i++) {
        parallel[i] = sin(toRadians(parallelVector[0] + initDegree + (360 / wheels) * i)) * parallelVector[1];
    }
    for(int i = 0; i < wheels - 1; i++) {
        parallelMax = fmax(parallel[i], parallel[i + 1]);
        parallelMin = fmin(parallel[i], parallel[i + 1]);
    }
    if(parallelMax + moment > 1.0 || parallelMin + moment < -1.0) {
        for(int i = 0; i < wheels; i++) {
            parallel[i] *= (1.0 - fabs(moment));
        }
    }
    for(int i = 0; i < wheels; i++) {
        wheel[i] = parallel[i] + moment;
    }
    return 1;
}

bool Omni::stop()
{
    for(int i = 0; i < wheels; i++) {
        wheel[i] = 0;
    }
    return 1;
}

void Omni::setWheels(int wheel)
{
    wheels = wheel;
}

void Omni::setInitDegree(double degree)
{
    initDegree = degree;
}

double Omni::getOutput(int num)
{
    return wheel[num];
}