Shuto Naruse / geometryfuncs

geometryfuncs for ICRS eurobot

geometryfuncs.h

Committer:
narshu
Date:
2012-04-26
Revision:
0:886118d8488d

File content as of revision 0:886118d8488d:

#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