Oskar Weigl / Mbed 2 deprecated Eurobot2013

We are going to win! wohoo

Dependencies:   mbed mbed-rtos

system/geometryfuncs.h

Committer:
madcowswe
Date:
2012-11-14
Revision:
9:08552997b544
Parent:
6:5a52c046d8f7

File content as of revision 9:08552997b544:

#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