Pascal Serrarens
a2ba625bac
All checks were successful
Build and Run C++ Unit Tests / build-and-test (push) Successful in 2m19s
384 lines
10 KiB
C++
384 lines
10 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"
|
|
#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;
|
|
}
|
|
*/
|
|
|
|
#pragma region Vector2Of
|
|
|
|
template <typename T>
|
|
Vector2Of<T>::Vector2Of() {}
|
|
|
|
template <typename T>
|
|
Vector2Of<T>::Vector2Of(T horizontal, T vertical)
|
|
: horizontal(horizontal), vertical(vertical) {}
|
|
|
|
template <typename T>
|
|
Vector2Of<float> Vector2Of<T>::FromPolar(PolarOf<T> p) {
|
|
float horizontalRad = p.angle.InDegrees() * Deg2Rad;
|
|
float cosHorizontal = cosf(horizontalRad);
|
|
float sinHorizontal = sinf(horizontalRad);
|
|
|
|
Vector2Of<float> v;
|
|
v.horizontal = p.distance * sinHorizontal;
|
|
v.vertical = p.distance * cosHorizontal;
|
|
return v;
|
|
}
|
|
|
|
template <typename T>
|
|
const Vector2Of<T> Vector2Of<T>::zero = Vector2Of(T{}, T{});
|
|
|
|
template <typename T>
|
|
const Vector2Of<T> Vector2Of<T>::forward = Vector2Of(T{}, 1);
|
|
|
|
template <typename T>
|
|
bool LinearAlgebra::Vector2Of<T>::operator==(const Vector2Of& v) {
|
|
return (this->horizontal == v.horizontal && this->vertical == v.vertical);
|
|
}
|
|
|
|
template <typename T>
|
|
float Vector2Of<T>::MagnitudeOf(const Vector2Of& v) {
|
|
T sqr = v.horizontal * v.horizontal + v.vertical * v.vertical;
|
|
float sqrFloat = static_cast<float>(sqr);
|
|
return sqrtf(sqrFloat);
|
|
}
|
|
|
|
template <typename T>
|
|
float Vector2Of<T>::Magnitude() const {
|
|
T sqr = this->horizontal * this->horizontal + this->vertical * this->vertical;
|
|
float sqrFloat = static_cast<float>(sqr);
|
|
return sqrtf(sqrFloat);
|
|
}
|
|
|
|
template <typename T>
|
|
float Vector2Of<T>::SqrMagnitudeOf(const Vector2Of& v) {
|
|
T sqr = v.horizontal * v.horizontal + v.vertical * v.vertical;
|
|
return static_cast<float>(sqr);
|
|
}
|
|
|
|
template <typename T>
|
|
float Vector2Of<T>::SqrMagnitude() const {
|
|
T sqr = this->horizontal * this->horizontal + this->vertical * this->vertical;
|
|
return static_cast<float>(sqr);
|
|
}
|
|
|
|
template <typename T>
|
|
Vector2Of<T> Vector2Of<T>::operator-() {
|
|
return Vector2Of<T>(-this->horizontal, -this->vertical);
|
|
}
|
|
|
|
template <typename T>
|
|
Vector2Of<T> Vector2Of<T>::operator-(const Vector2Of& v) const {
|
|
return Vector2Of(this->horizontal - v.horizontal,
|
|
this->vertical - v.vertical);
|
|
}
|
|
|
|
template <typename T>
|
|
Vector2Of<T> Vector2Of<T>::operator-=(const Vector2Of& v) {
|
|
this->horizontal -= v.horizontal;
|
|
this->vertical -= v.vertical;
|
|
return *this;
|
|
}
|
|
|
|
template <typename T>
|
|
Vector2Of<T> Vector2Of<T>::operator+(const Vector2Of& v) const {
|
|
return Vector2Of(this->horizontal + v.horizontal,
|
|
this->vertical + v.vertical);
|
|
}
|
|
|
|
template <typename T>
|
|
Vector2Of<T> Vector2Of<T>::operator+=(const Vector2Of& v) {
|
|
this->horizontal += v.horizontal;
|
|
this->vertical += v.vertical;
|
|
return *this;
|
|
}
|
|
|
|
template <typename T>
|
|
Vector2Of<T> Vector2Of<T>::Scale(const Vector2Of& v1, const Vector2Of& v2) {
|
|
return Vector2Of<T>(v1.horizontal * v2.horizontal, v1.vertical * v2.vertical);
|
|
}
|
|
|
|
// template <typename T>
|
|
// Vector2Of<float> Vector2Of<T>::operator/=(float f) {
|
|
// this->x /= f;
|
|
// this->y /= f;
|
|
// return *this;
|
|
// }
|
|
|
|
template <typename T>
|
|
T Vector2Of<T>::Dot(const Vector2Of& v1, const Vector2Of& v2) {
|
|
return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical;
|
|
}
|
|
|
|
template <typename T>
|
|
float Vector2Of<T>::Distance(const Vector2Of& v1, const Vector2Of& v2) {
|
|
return MagnitudeOf(v1 - v2);
|
|
}
|
|
|
|
template <typename T>
|
|
Vector2Of<float> Vector2Of<T>::Normalize(const Vector2Of<T>& v) {
|
|
float num = Vector2Of<T>::MagnitudeOf(v);
|
|
Vector2Of<float> result = Vector2Of::zero;
|
|
if (num > Float::epsilon) {
|
|
result = static_cast<Vector2Of<float>>(v) / num;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
template <typename T>
|
|
AngleOf<T> Vector2Of<T>::UnsignedAngle(const Vector2Of& v1,
|
|
const Vector2Of& v2) {
|
|
return AngleOf<T>::Abs(SignedAngle(v1, v2));
|
|
}
|
|
|
|
template <typename T>
|
|
AngleOf<T> Vector2Of<T>::SignedAngle(const Vector2Of& v1, const Vector2Of& v2) {
|
|
float sqrMagFrom = v1.SqrMagnitude();
|
|
float sqrMagTo = v2.SqrMagnitude();
|
|
|
|
if (sqrMagFrom == 0 || sqrMagTo == 0)
|
|
return AngleOf<T>::zero;
|
|
if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo))
|
|
return AngleOf<T>::zero; // Angle does not support NaN...
|
|
|
|
AngleOf<T> angleFrom = AngleOf<T>::Atan2(static_cast<float>(v1.vertical),
|
|
static_cast<float>(v1.horizontal));
|
|
AngleOf<T> angleTo = AngleOf<T>::Atan2(static_cast<float>(v2.vertical),
|
|
static_cast<float>(v2.horizontal));
|
|
return -(angleTo - angleFrom);
|
|
}
|
|
|
|
template <typename T>
|
|
Vector2Of<float> Vector2Of<T>::Rotate(const Vector2Of& v, AngleOf<T> a) {
|
|
float sinValue = AngleOf<T>::Sin(a);
|
|
float cosValue = AngleOf<T>::Cos(a);
|
|
float tx = static_cast<float>(v.horizontal);
|
|
float ty = static_cast<float>(v.vertical);
|
|
Vector2Of<float> r = Vector2Of<float>((cosValue * tx) - (sinValue * ty),
|
|
(sinValue * tx) + (cosValue * ty));
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
Vector2Of<float> Vector2Of<T>::Lerp(const Vector2Of& v1,
|
|
const Vector2Of& v2,
|
|
float f) {
|
|
Vector2Of<float> v = v1 + static_cast<Vector2Of<float>>(v2 - v1) * f;
|
|
return v;
|
|
}
|
|
|
|
// 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;
|
|
// }
|
|
|
|
template <typename T>
|
|
Vector2Of<float> Vector2Of<T>::Normalized() const {
|
|
float num = Vector2Of<T>::MagnitudeOf(*this);
|
|
Vector2Of<float> result = Vector2Of::zero;
|
|
if (num > Float::epsilon) {
|
|
result = static_cast<Vector2Of<float>>(*this) / num;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
// Explicit instantiation for int
|
|
template class Vector2Of<signed short>;
|
|
template class Vector2Of<float>;
|
|
|
|
#pragma endregion Vector2Of |