146 lines
3.5 KiB
C++
146 lines
3.5 KiB
C++
// 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"
|
|
|
|
#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() {}
|
|
|
|
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);
|
|
|
|
float Vector2::Magnitude(const Vector2 &a) {
|
|
return sqrtf(a.x * a.x + a.y * a.y);
|
|
}
|
|
float Vector2::magnitude() const { return (float)sqrtf(x * x + y * y); }
|
|
|
|
float Vector2::SqrMagnitude(const Vector2 &a) { return a.x * a.x + a.y * a.y; }
|
|
float Vector2::sqrMagnitude() const { return (x * x + y * y); }
|
|
|
|
Vector2 Vector2::Normalize(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-(const Vector2 &v2) const {
|
|
return Vector2(this->x - v2.x, this->y - v2.y);
|
|
}
|
|
|
|
Vector2 Vector2::operator-() { return Vector2(-this->x, -this->y); }
|
|
|
|
Vector2 Vector2::operator+(const Vector2 &v2) const {
|
|
return Vector2(this->x + v2.x, this->y + v2.y);
|
|
}
|
|
|
|
Vector2 Vector2::Scale(const Vector2 &p1, const Vector2 &p2) {
|
|
return Vector2(p1.x * p2.x, p1.y * p2.y);
|
|
}
|
|
|
|
Vector2 Vector2::operator*(float f) const {
|
|
return Vector2(this->x * f, this->y * f);
|
|
}
|
|
|
|
Vector2 Vector2::operator/(const float &d) {
|
|
return Vector2(this->x / d, this->y / d);
|
|
}
|
|
|
|
float Vector2::Dot(const Vector2 &v1, const Vector2 &v2) {
|
|
return v1.x * v2.x + v1.y * v2.y;
|
|
}
|
|
|
|
bool Vector2::operator==(const Vector2 &v) {
|
|
return (this->x == v.x && this->y == v.y);
|
|
}
|
|
|
|
float Vector2::Distance(const Vector2 &p1, const Vector2 &p2) {
|
|
return Magnitude(p1 - p2);
|
|
}
|
|
|
|
float Vector2::Angle(Vector2 from, Vector2 to) {
|
|
return (float)fabs(SignedAngle(from, to));
|
|
}
|
|
|
|
float Vector2::SignedAngle(Vector2 from, Vector2 to) {
|
|
float sqrMagFrom = from.sqrMagnitude();
|
|
float sqrMagTo = to.sqrMagnitude();
|
|
|
|
if (sqrMagFrom == 0 || sqrMagTo == 0)
|
|
return 0;
|
|
if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo))
|
|
#if defined(AVR)
|
|
return NAN;
|
|
#else
|
|
return nanf("");
|
|
#endif
|
|
|
|
float angleFrom = atan2(from.y, from.x);
|
|
float angleTo = atan2(to.y, to.x);
|
|
return (angleTo - angleFrom) * Angle::Rad2Deg;
|
|
}
|
|
|
|
Vector2 Vector2::Rotate(Vector2 v, float angle) {
|
|
#if defined(AVR)
|
|
float sinValue = sin(angle * Angle::Deg2Rad);
|
|
float cosValue = cos(angle * Angle::Deg2Rad);
|
|
#else
|
|
float sinValue = (float)sinf(angle * Angle::Deg2Rad);
|
|
float cosValue = (float)cosf(angle * Angle::Deg2Rad);
|
|
#endif
|
|
|
|
float tx = v.x;
|
|
float ty = v.y;
|
|
v.x = (cosValue * tx) - (sinValue * ty);
|
|
v.y = (sinValue * tx) + (cosValue * ty);
|
|
return v;
|
|
}
|
|
|
|
Vector2 Vector2::Lerp(Vector2 from, Vector2 to, float f) {
|
|
Vector2 v = from + (to - from) * f;
|
|
return v;
|
|
}
|
|
|
|
float Vector2::ToFactor(Vector2 a, Vector2 b) {
|
|
return (1 - Vector2::Dot(a, b)) / 2;
|
|
}
|