Shuto Naruse / Eurobot_2012_shared

Eurobot_shared pubulished from Eurobot Primary

geometryfuncs/geometryfuncs.h

Committer:
narshu
Date:
2012-08-07
Revision:
0:434fd09723be

File content as of revision 0:434fd09723be:

#ifndef GEOMETRYFUNCS_H
#define GEOMETRYFUNCS_H

#include <tvmet/Matrix.h>

template <typename T>
Matrix <T, 2, 2> Rotmatrix(T theta) {
     Matrix <T, 2, 2> outmatrix;
     outmatrix = cos(theta), -sin(theta),
                 sin(theta), cos(theta);
     return outmatrix;
}

// rectifies angle to range -PI to PI
template <typename T>
T rectifyAng (T ang_in) {
    ang_in -= (floor(ang_in/(2*PI)))*2*PI;
    if (ang_in < -PI) {
        ang_in += 2*PI;
    }
    if (ang_in > PI) {
        ang_in -= 2*PI;
    }

    return ang_in;
}

#endif //GEOMETRYFUNCS_H