Shuto Naruse / Mbed 2 deprecated Eurobot_2012_Secondary

Eurobot_2012_Secondary

Dependencies:   mbed tvmet

geometryfuncs.h

Committer:
narshu
Date:
2012-04-20
Revision:
0:fbfafa6bf5f9

File content as of revision 0:fbfafa6bf5f9:

#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