robot/Vector.h at main · Athena-Robotics/robot · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
#include "Arduino.h"
#include "HardwareSerial.h"
#pragma once

#ifndef VECTOR_H
#define VECTOR_H

#include "Heading.h"

class StrategyVector {
public:
  float x, y;
  float theta, magnitude;

  StrategyVector() {
    x = 0;
    y = 0;
    theta = 0;
    magnitude = 0;
  }

  StrategyVector(Heading direction, float magnitude) {
    float deg = direction.as_radians();

    x = magnitude * cos(deg);
    y = magnitude * sin(deg);

    this->theta = deg;
    this->magnitude = magnitude;
  }

  float get_x() {
    return x;
  }

  float get_y() {
    return y;
  }

  friend StrategyVector operator+(StrategyVector lhs, StrategyVector rhs) {
    float res_x = lhs.get_x() + rhs.get_x();
    float res_y = lhs.get_y() + rhs.get_y();

    float magnitude = sqrt(res_x * res_x + res_y * res_y);

    StrategyVector resultant_vector = StrategyVector(atan2(res_y, res_x), magnitude);
    return resultant_vector;
  }

  friend StrategyVector operator-(StrategyVector lhs, StrategyVector rhs) {
    float res_x = lhs.get_x() - rhs.get_x();
    float res_y = lhs.get_y() - rhs.get_y();

    float magnitude = sqrt(res_x * res_x + res_y * res_y);

    StrategyVector resultant_vector = StrategyVector(atan2(res_y, res_x), magnitude);
    return resultant_vector;
  }

  float get_magnitude() {
    return sqrtf(x * x + y * y);
  }

  void normalise() {
    magnitude = this->get_magnitude();
    x /= magnitude;
    y /= magnitude;
  }

  friend StrategyVector operator*(StrategyVector vec, float rhs) {
    float res_x = vec.get_x();
    float res_y = vec.get_y();

    float magnitude = sqrt(res_x * res_x + res_y * res_y) * rhs;

    StrategyVector resultant_vector = StrategyVector(atan2(res_y, res_x), magnitude);
    return resultant_vector;
  }

  friend StrategyVector operator*(float rhs, StrategyVector vec) {
    float res_x = vec.get_x();
    float res_y = vec.get_y();

    float magnitude = sqrt(res_x * res_x + res_y * res_y) * rhs;

    StrategyVector resultant_vector = StrategyVector(atan2(res_y, res_x), magnitude);
    return resultant_vector;
  }

  Heading to_heading() {
    return Heading(theta);
  }

  void SerialPrint() {
    Serial.print("Dir: ");
    Serial.print(theta * RAD_TO_DEG);
    Serial.print(" | Mag: ");
    Serial.print(magnitude);
  }
};

#endif  // VECTOR_H