RoboidControl-cpp/LinearAlgebra/Vector2.cpp

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.

#include "Vector2.h"
#include "Angle.h"
#include "FloatSingle.h"
#include "Vector3.h"

// #if defined(AVR)
// #include <Arduino.h>
// #else
#include <math.h>
// #endif

Vector2::Vector2() {
  x = 0;
  y = 0;
}
Vector2::Vector2(float _x, float _y) {
  x = _x;
  y = _y;
}
// Vector2::Vector2(Vec2 v) {
//   x = v.x;
//   y = v.y;
// }
Vector2::Vector2(Vector3 v) {
  x = v.Right();    // x;
  y = v.Forward();  // z;
}
Vector2::Vector2(PolarSingle p) {
  float horizontalRad = p.angle.InDegrees() * Deg2Rad;
  float cosHorizontal = cosf(horizontalRad);
  float sinHorizontal = sinf(horizontalRad);

  x = p.distance * sinHorizontal;
  y = p.distance * cosHorizontal;
}

Vector2::~Vector2() {}

const Vector2 Vector2::zero = Vector2(0, 0);
const Vector2 Vector2::one = Vector2(1, 1);
const Vector2 Vector2::right = Vector2(1, 0);
const Vector2 Vector2::left = Vector2(-1, 0);
const Vector2 Vector2::up = Vector2(0, 1);
const Vector2 Vector2::down = Vector2(0, -1);
const Vector2 Vector2::forward = Vector2(0, 1);
const Vector2 Vector2::back = Vector2(0, -1);

bool Vector2::operator==(const Vector2& v) {
  return (this->x == v.x && this->y == v.y);
}

float Vector2::Magnitude(const Vector2& v) {
  return sqrtf(v.x * v.x + v.y * v.y);
}
float Vector2::magnitude() const {
  return (float)sqrtf(x * x + y * y);
}
float Vector2::SqrMagnitude(const Vector2& v) {
  return v.x * v.x + v.y * v.y;
}
float Vector2::sqrMagnitude() const {
  return (x * x + y * y);
}

Vector2 Vector2::Normalize(const Vector2& v) {
  float num = Vector2::Magnitude(v);
  Vector2 result = Vector2::zero;
  if (num > Float::epsilon) {
    result = v / num;
  }
  return result;
}
Vector2 Vector2::normalized() const {
  float num = this->magnitude();
  Vector2 result = Vector2::zero;
  if (num > Float::epsilon) {
    result = ((Vector2) * this) / num;
  }
  return result;
}

Vector2 Vector2::operator-() {
  return Vector2(-this->x, -this->y);
}

Vector2 Vector2::operator-(const Vector2& v) const {
  return Vector2(this->x - v.x, this->y - v.y);
}
Vector2 Vector2::operator-=(const Vector2& v) {
  this->x -= v.x;
  this->y -= v.y;
  return *this;
}
Vector2 Vector2::operator+(const Vector2& v) const {
  return Vector2(this->x + v.x, this->y + v.y);
}
Vector2 Vector2::operator+=(const Vector2& v) {
  this->x += v.x;
  this->y += v.y;
  return *this;
}

Vector2 Vector2::Scale(const Vector2& v1, const Vector2& v2) {
  return Vector2(v1.x * v2.x, v1.y * v2.y);
}
// Vector2 Passer::LinearAlgebra::operator*(const Vector2 &v, float f) {
//   return Vector2(v.x * f, v.y * f);
// }
// Vector2 Passer::LinearAlgebra::operator*(float f, const Vector2 &v) {
//   return Vector2(v.x * f, v.y * f);
// }
Vector2 Vector2::operator*=(float f) {
  this->x *= f;
  this->y *= f;
  return *this;
}
// Vector2 Passer::LinearAlgebra::operator/(const Vector2 &v, float f) {
//   return Vector2(v.x / f, v.y / f);
// }
// Vector2 Passer::LinearAlgebra::operator/(float f, const Vector2 &v) {
//   return Vector2(v.x / f, v.y / f);
// }
Vector2 Vector2::operator/=(float f) {
  this->x /= f;
  this->y /= f;
  return *this;
}

float Vector2::Dot(const Vector2& v1, const Vector2& v2) {
  return v1.x * v2.x + v1.y * v2.y;
}

float Vector2::Distance(const Vector2& v1, const Vector2& v2) {
  return Magnitude(v1 - v2);
}

float Vector2::Angle(const Vector2& v1, const Vector2& v2) {
  return (float)fabs(SignedAngle(v1, v2));
}
float Vector2::SignedAngle(const Vector2& v1, const Vector2& v2) {
  float sqrMagFrom = v1.sqrMagnitude();
  float sqrMagTo = v2.sqrMagnitude();

  if (sqrMagFrom == 0 || sqrMagTo == 0)
    return 0;
  if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo))
#if defined(AVR)
    return NAN;
#else
    return nanf("");
#endif

  float angleFrom = atan2f(v1.y, v1.x);
  float angleTo = atan2f(v2.y, v2.x);
  return -(angleTo - angleFrom) * Rad2Deg;
}

Vector2 Vector2::Rotate(const Vector2& v, AngleSingle a) {
  float angleRad = a.InDegrees() * Deg2Rad;
#if defined(AVR)
  float sinValue = sin(angleRad);
  float cosValue = cos(angleRad);  // * Angle::Deg2Rad);
#else
  float sinValue = (float)sinf(angleRad);
  float cosValue = (float)cosf(angleRad);
#endif

  float tx = v.x;
  float ty = v.y;
  Vector2 r = Vector2((cosValue * tx) - (sinValue * ty),
                      (sinValue * tx) + (cosValue * ty));
  return r;
}

Vector2 Vector2::Lerp(const Vector2& v1, const Vector2& v2, float f) {
  Vector2 v = v1 + (v2 - v1) * f;
  return v;
}