JTwoDTransform.h

Go to the documentation of this file.

#ifndef J_TWO_D_TRANSFORM_H
#define J_TWO_D_TRANSFORM_H
#include <Arduino.h>
struct JTwoDTransform {
    float x;
    float y;
    float theta;
 
    static JTwoDTransform rotate(JTwoDTransform tIn, float theta)
    {
        return { tIn.x * cos(-theta) - tIn.y * sin(-theta), tIn.x * sin(-theta) + tIn.y * cos(-theta), tIn.theta };
    }
 
    static JTwoDTransform fromNice(float fb, float sideways, float turn)
    {
        return { fb, -sideways, (float)radians(-turn) };
    }
 
    static float getForwards(JTwoDTransform t)
    {
        return t.x;
    }
 
    static float getRight(JTwoDTransform t)
    {
        return -t.y;
    }
 
    static float getTurnDegreesCW(JTwoDTransform t)
    {
        return -degrees(t.theta);
    }
 
    JTwoDTransform operator&(const JTwoDTransform& t)
    {
 
        this->y = this->y + cos(this->theta) * t.y + sin(this->theta) * t.x;
        this->x = this->x + cos(this->theta) * t.x + sin(this->theta) * t.y;
        this->theta = this->theta + t.theta;
        return *this;
    }
 
    JTwoDTransform operator+(const JTwoDTransform& t)
    {
        this->y = this->y + t.y;
        this->theta = this->theta + t.theta;
        this->x = this->x + t.x;
        return *this;
    }
    JTwoDTransform operator-(const JTwoDTransform& t)
    {
        this->y = this->y - t.y;
        this->theta = this->theta - t.theta;
        this->x = this->x - t.x;
        return *this;
    }
    JTwoDTransform operator*(const JTwoDTransform& t)
    {
        this->y = this->y * t.y;
        this->theta = this->theta * t.theta;
        this->x = this->x * t.x;
        return *this;
    }
    JTwoDTransform operator/(const JTwoDTransform& t)
    {
        this->y = this->y / t.y;
        this->theta = this->theta / t.theta;
        this->x = this->x / t.x;
        return *this;
    }
    JTwoDTransform operator*(const float v)
    {
        this->y = this->y * v;
        this->theta = this->theta * v;
        this->x = this->x * v;
        return *this;
    }
    JTwoDTransform operator/(const float v)
    {
        this->y = this->y / v;
        this->theta = this->theta / v;
        this->x = this->x / v;
        return *this;
    }
    JTwoDTransform operator-()
    {
        this->y = -this->y;
        this->theta = -this->theta;
        this->x = -this->x;
        return *this;
    }
    JTwoDTransform operator+=(const JTwoDTransform& t)
    {
        this->y += t.y;
        this->theta += t.theta;
        this->x += t.x;
        return *this;
    }
    JTwoDTransform operator-=(const JTwoDTransform& t)
    {
        this->y -= t.y;
        this->theta -= t.theta;
        this->x -= t.x;
        return *this;
    }
    JTwoDTransform operator*=(const JTwoDTransform& t)
    {
        this->y *= t.y;
        this->theta *= t.theta;
        this->x *= t.x;
        return *this;
    }
    JTwoDTransform operator/=(const JTwoDTransform& t)
    {
        this->y /= t.y;
        this->theta /= t.theta;
        this->x /= t.x;
        return *this;
    }
    JTwoDTransform operator*=(const float v)
    {
        this->y *= v;
        this->theta *= v;
        this->x *= v;
        return *this;
    }
    JTwoDTransform operator/=(const float v)
    {
        this->y /= v;
        this->theta /= v;
        this->x /= v;
        return *this;
    }
 
    float getByNumber(int8_t index)
    {
        if (index == 0)
            return this->x;
        if (index == 1)
            return this->y;
        if (index == 2)
            return this->theta;
        return NAN;
    }
 
    void setByNumber(int8_t index, float value)
    {
        if (index == 0)
            this->x = value;
        if (index == 1)
            this->y = value;
        if (index == 2)
            this->theta = value;
    }
 
    float sumAbs()
    {
        return abs(this->y) + abs(this->x) + abs(this->theta);
    }
    void print(bool labels = true, bool newline = false, Stream* serial = &Serial)
    {
        if (labels)
            serial->print("x_fb:");
        serial->print(x, 5);
        serial->print(" ");
        if (labels)
            serial->print("y_lr:");
        serial->print(y, 5);
        serial->print(" ");
        if (labels)
            serial->print("rz_ccw:");
        serial->print(theta, 5);
        serial->print(" ");
        if (newline)
            serial->print("\n");
    }
};
#endif