v1.4.4
This commit is contained in:
Aria
commit
9c94d113d3
GPG key ID:
19AB7AA462B8AB3B
10260 changed files with 1237388 additions and 0 deletions
925
source/core/StarVector.hpp
Normal file
925
source/core/StarVector.hpp
Normal file
|
|
@ -0,0 +1,925 @@
|
|||
#ifndef STAR_VECTOR_HPP
|
||||
#define STAR_VECTOR_HPP
|
||||
|
||||
#include "StarArray.hpp"
|
||||
#include "StarMathCommon.hpp"
|
||||
#include "StarAlgorithm.hpp"
|
||||
#include "StarHash.hpp"
|
||||
|
||||
namespace Star {
|
||||
|
||||
template <typename T, size_t N>
|
||||
class Vector : public Array<T, N> {
|
||||
public:
|
||||
typedef Array<T, N> Base;
|
||||
|
||||
template <size_t P, typename T2 = void>
|
||||
using Enable2D = typename std::enable_if<P == 2 && N == P, T2>::type;
|
||||
|
||||
template <size_t P, typename T2 = void>
|
||||
using Enable3D = typename std::enable_if<P == 3 && N == P, T2>::type;
|
||||
|
||||
template <size_t P, typename T2 = void>
|
||||
using Enable4D = typename std::enable_if<P == 4 && N == P, T2>::type;
|
||||
|
||||
template <size_t P, typename T2 = void>
|
||||
using Enable2DOrHigher = typename std::enable_if<P >= 2 && N == P, T2>::type;
|
||||
|
||||
template <size_t P, typename T2 = void>
|
||||
using Enable3DOrHigher = typename std::enable_if<P >= 3 && N == P, T2>::type;
|
||||
|
||||
template <size_t P, typename T2 = void>
|
||||
using Enable4DOrHigher = typename std::enable_if<P >= 4 && N == P, T2>::type;
|
||||
|
||||
static Vector filled(T const& t);
|
||||
|
||||
template <typename T2>
|
||||
static Vector floor(Vector<T2, N> const& v);
|
||||
|
||||
template <typename T2>
|
||||
static Vector ceil(Vector<T2, N> const& v);
|
||||
|
||||
template <typename T2>
|
||||
static Vector round(Vector<T2, N> const& v);
|
||||
|
||||
template <typename Iterator>
|
||||
static Vector copyFrom(Iterator p);
|
||||
|
||||
// Is zero-initialized (from Array)
|
||||
Vector();
|
||||
|
||||
explicit Vector(T const& e1);
|
||||
|
||||
template <typename... TN>
|
||||
Vector(T const& e1, TN const&... rest);
|
||||
|
||||
template <typename T2>
|
||||
explicit Vector(Array<T2, N> const& v);
|
||||
|
||||
template <typename T2, typename T3>
|
||||
Vector(Array<T2, N - 1> const& u, T3 const& v);
|
||||
|
||||
template <size_t N2>
|
||||
Vector<T, N2> toSize() const;
|
||||
Vector<T, 2> vec2() const;
|
||||
Vector<T, 3> vec3() const;
|
||||
Vector<T, 4> vec4() const;
|
||||
|
||||
Vector piecewiseMultiply(Vector const& v2) const;
|
||||
Vector piecewiseDivide(Vector const& v2) const;
|
||||
|
||||
Vector piecewiseMin(Vector const& v2) const;
|
||||
Vector piecewiseMax(Vector const& v2) const;
|
||||
Vector piecewiseClamp(Vector const& min, Vector const& max) const;
|
||||
|
||||
T min() const;
|
||||
T max() const;
|
||||
|
||||
T sum() const;
|
||||
T product() const;
|
||||
|
||||
template <typename Function>
|
||||
Vector combine(Vector const& v, Function f) const;
|
||||
|
||||
// Outputs angles in the range [0, pi]
|
||||
T angleBetween(Vector const& v) const;
|
||||
|
||||
// Angle between two normalized vectors.
|
||||
T angleBetweenNormalized(Vector const& v) const;
|
||||
|
||||
T magnitudeSquared() const;
|
||||
T magnitude() const;
|
||||
|
||||
void normalize();
|
||||
Vector normalized() const;
|
||||
|
||||
Vector projectOnto(Vector const& v) const;
|
||||
|
||||
Vector projectOntoNormalized(Vector const& v) const;
|
||||
|
||||
void negate();
|
||||
|
||||
// Reverses order of components of vector
|
||||
void reverse();
|
||||
|
||||
Vector abs() const;
|
||||
Vector floor() const;
|
||||
Vector ceil() const;
|
||||
Vector round() const;
|
||||
|
||||
void fill(T const& v);
|
||||
void clamp(T const& min, T const& max);
|
||||
|
||||
template <typename Function>
|
||||
void transform(Function&& function);
|
||||
|
||||
template <typename Function>
|
||||
Vector<decltype(std::declval<Function>()(std::declval<T>())), N> transformed(Function&& function) const;
|
||||
|
||||
Vector operator-() const;
|
||||
|
||||
Vector operator+(Vector const& v) const;
|
||||
Vector operator-(Vector const& v) const;
|
||||
T operator*(Vector const& v) const;
|
||||
Vector operator*(T s) const;
|
||||
Vector operator/(T s) const;
|
||||
Vector& operator+=(Vector const& v);
|
||||
Vector& operator-=(Vector const& v);
|
||||
Vector& operator*=(T s);
|
||||
Vector& operator/=(T s);
|
||||
|
||||
// Vector2
|
||||
|
||||
// Return vector rotated to given angle
|
||||
template <size_t P = N>
|
||||
static Enable2D<P, Vector> withAngle(T angle, T magnitude = 1);
|
||||
|
||||
template <size_t P = N>
|
||||
static Enable2D<P, T> angleBetween2(Vector const& u, Vector const& v);
|
||||
template <size_t P = N>
|
||||
static Enable2D<P, T> angleFormedBy2(Vector const& a, Vector const& b, Vector const& c);
|
||||
template <size_t P = N>
|
||||
static Enable2D<P, T> angleFormedBy2(Vector const& a, Vector const& b, Vector const& c, std::function<Vector(Vector, Vector)> const& diff);
|
||||
|
||||
template <size_t P = N>
|
||||
Enable2D<P, Vector> rotate(T angle) const;
|
||||
|
||||
// Faster than rotate(Constants::pi/2).
|
||||
template <size_t P = N>
|
||||
Enable2D<P, Vector> rot90() const;
|
||||
|
||||
// Angle of vector on 2d plane, in the range [-pi, pi]
|
||||
template <size_t P = N>
|
||||
Enable2D<P, T> angle() const;
|
||||
|
||||
// Returns polar coordinates of this cartesian vector
|
||||
template <size_t P = N>
|
||||
Enable2D<P, Vector> toPolar() const;
|
||||
|
||||
// Returns cartesian coordinates of this polar vector
|
||||
template <size_t P = N>
|
||||
Enable2D<P, Vector> toCartesian() const;
|
||||
|
||||
template <size_t P = N>
|
||||
Enable2DOrHigher<P, T> const& x() const;
|
||||
template <size_t P = N>
|
||||
Enable2DOrHigher<P, T> const& y() const;
|
||||
|
||||
template <size_t P = N>
|
||||
Enable2DOrHigher<P> setX(T const& t);
|
||||
template <size_t P = N>
|
||||
Enable2DOrHigher<P> setY(T const& t);
|
||||
|
||||
// Vector3
|
||||
|
||||
template <size_t P = N>
|
||||
static Enable3D<P, Vector> fromAngles(T psi, T theta);
|
||||
template <size_t P = N>
|
||||
static Enable3D<P, Vector> fromAnglesEnu(T psi, T theta);
|
||||
template <size_t P = N>
|
||||
static Enable3D<P, T> tripleScalarProduct(Vector const& u, Vector const& v, Vector const& w);
|
||||
template <size_t P = N>
|
||||
static Enable3D<P, T> angle(Vector const& v1, Vector const& v2);
|
||||
|
||||
template <size_t P = N>
|
||||
Enable3D<P, T> psi() const;
|
||||
template <size_t P = N>
|
||||
Enable3D<P, T> theta() const;
|
||||
template <size_t P = N>
|
||||
Enable3D<P, Vector<T, 2>> eulers() const;
|
||||
|
||||
template <size_t P = N>
|
||||
Enable3D<P, T> psiEnu() const;
|
||||
template <size_t P = N>
|
||||
Enable3D<P, T> thetaEnu() const;
|
||||
|
||||
template <size_t P = N>
|
||||
Enable3D<P, Vector> nedToEnu() const;
|
||||
template <size_t P = N>
|
||||
Enable3D<P, Vector> enuToNed() const;
|
||||
|
||||
template <size_t P = N>
|
||||
Enable3DOrHigher<P, T> const& z() const;
|
||||
|
||||
template <size_t P = N>
|
||||
Enable3DOrHigher<P> setZ(T const& t);
|
||||
|
||||
// Vector4
|
||||
|
||||
template <size_t P = N>
|
||||
Enable4DOrHigher<P, T> const& w() const;
|
||||
|
||||
template <size_t P = N>
|
||||
Enable4DOrHigher<P> setW(T const& t);
|
||||
|
||||
private:
|
||||
using Base::size;
|
||||
using Base::empty;
|
||||
};
|
||||
|
||||
typedef Vector<int, 2> Vec2I;
|
||||
typedef Vector<unsigned, 2> Vec2U;
|
||||
typedef Vector<float, 2> Vec2F;
|
||||
typedef Vector<double, 2> Vec2D;
|
||||
typedef Vector<uint8_t, 2> Vec2B;
|
||||
typedef Vector<size_t, 2> Vec2S;
|
||||
|
||||
typedef Vector<int, 3> Vec3I;
|
||||
typedef Vector<unsigned, 3> Vec3U;
|
||||
typedef Vector<float, 3> Vec3F;
|
||||
typedef Vector<double, 3> Vec3D;
|
||||
typedef Vector<uint8_t, 3> Vec3B;
|
||||
typedef Vector<size_t, 3> Vec3S;
|
||||
|
||||
typedef Vector<int, 4> Vec4I;
|
||||
typedef Vector<unsigned, 4> Vec4U;
|
||||
typedef Vector<float, 4> Vec4F;
|
||||
typedef Vector<double, 4> Vec4D;
|
||||
typedef Vector<uint8_t, 4> Vec4B;
|
||||
typedef Vector<size_t, 4> Vec4S;
|
||||
|
||||
template <typename T, size_t N>
|
||||
std::ostream& operator<<(std::ostream& os, Vector<T, N> const& v);
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> operator*(T s, Vector<T, N> v);
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> vnorm(Vector<T, N> v);
|
||||
|
||||
template <typename T, size_t N>
|
||||
T vmag(Vector<T, N> const& v);
|
||||
|
||||
template <typename T, size_t N>
|
||||
T vmagSquared(Vector<T, N> const& v);
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> vmin(Vector<T, N> const& a, Vector<T, N> const& b);
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> vmax(Vector<T, N> const& a, Vector<T, N> const& b);
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> vclamp(Vector<T, N> const& a, Vector<T, N> const& min, Vector<T, N> const& max);
|
||||
|
||||
template <typename VectorType>
|
||||
VectorType vmult(VectorType const& a, VectorType const& b);
|
||||
|
||||
template <typename VectorType>
|
||||
VectorType vdiv(VectorType const& a, VectorType const& b);
|
||||
|
||||
// Returns the cross product
|
||||
template <typename T>
|
||||
Vector<T, 3> operator^(Vector<T, 3> v1, Vector<T, 3> v2);
|
||||
|
||||
// Returns the cross product / determinant
|
||||
template <typename T>
|
||||
T operator^(Vector<T, 2> const& v1, Vector<T, 2> const& v2);
|
||||
|
||||
template <typename T, size_t N>
|
||||
struct hash<Vector<T, N>> : hash<Array<T, N>> {};
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::filled(T const& t) {
|
||||
Vector v;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
v[i] = t;
|
||||
return v;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <typename T2>
|
||||
Vector<T, N> Vector<T, N>::floor(Vector<T2, N> const& v) {
|
||||
Vector vec;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
vec[i] = Star::floor(v[i]);
|
||||
return vec;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <typename T2>
|
||||
Vector<T, N> Vector<T, N>::ceil(Vector<T2, N> const& v) {
|
||||
Vector vec;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
vec[i] = Star::ceil(v[i]);
|
||||
return vec;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <typename T2>
|
||||
Vector<T, N> Vector<T, N>::round(Vector<T2, N> const& v) {
|
||||
Vector vec;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
vec[i] = Star::round(v[i]);
|
||||
return vec;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <typename Iterator>
|
||||
Vector<T, N> Vector<T, N>::copyFrom(Iterator p) {
|
||||
Vector v;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
v[i] = *(p++);
|
||||
return v;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N>::Vector() {}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N>::Vector(T const& e1)
|
||||
: Base(e1) {}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <typename... TN>
|
||||
Vector<T, N>::Vector(T const& e1, TN const&... rest)
|
||||
: Base(e1, rest...) {}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <typename T2>
|
||||
Vector<T, N>::Vector(Array<T2, N> const& v)
|
||||
: Base(v) {}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <typename T2, typename T3>
|
||||
Vector<T, N>::Vector(Array<T2, N - 1> const& u, T3 const& v) {
|
||||
for (size_t i = 0; i < N - 1; ++i) {
|
||||
Base::operator[](i) = u[i];
|
||||
}
|
||||
Base::operator[](N - 1) = v;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t N2>
|
||||
Vector<T, N2> Vector<T, N>::toSize() const {
|
||||
Vector<T, N2> r;
|
||||
size_t ns = Star::min(N2, N);
|
||||
for (size_t i = 0; i < ns; ++i)
|
||||
r[i] = (*this)[i];
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, 2> Vector<T, N>::vec2() const {
|
||||
return toSize<2>();
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, 3> Vector<T, N>::vec3() const {
|
||||
return toSize<3>();
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, 4> Vector<T, N>::vec4() const {
|
||||
return toSize<4>();
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::piecewiseMultiply(Vector const& v2) const {
|
||||
return combine(v2, std::multiplies<T>());
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::piecewiseDivide(Vector const& v2) const {
|
||||
return combine(v2, std::divides<T>());
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::piecewiseMin(Vector const& v2) const {
|
||||
Vector r;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
r[i] = Star::min((*this)[i], v2[i]);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::piecewiseMax(Vector const& v2) const {
|
||||
Vector r;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
r[i] = Star::max((*this)[i], v2[i]);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::piecewiseClamp(Vector const& min, Vector const& max) const {
|
||||
Vector r;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
r[i] = Star::max(Star::min((*this)[i], max[i]), min[i]);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
T Vector<T, N>::min() const {
|
||||
T s = (*this)[0];
|
||||
for (size_t i = 1; i < N; ++i)
|
||||
s = Star::min(s, (*this)[i]);
|
||||
return s;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
T Vector<T, N>::max() const {
|
||||
T s = (*this)[0];
|
||||
for (size_t i = 1; i < N; ++i)
|
||||
s = Star::max(s, (*this)[i]);
|
||||
return s;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
T Vector<T, N>::sum() const {
|
||||
T s = (*this)[0];
|
||||
for (size_t i = 1; i < N; ++i)
|
||||
s += (*this)[i];
|
||||
return s;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
T Vector<T, N>::product() const {
|
||||
T p = (*this)[0];
|
||||
for (size_t i = 1; i < N; ++i)
|
||||
p *= (*this)[i];
|
||||
return p;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <typename Function>
|
||||
Vector<T, N> Vector<T, N>::combine(Vector const& v, Function f) const {
|
||||
Vector r;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
r[i] = f((*this)[i], v[i]);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
T Vector<T, N>::angleBetween(Vector const& v) const {
|
||||
return acos(this->normalized() * v.normalized());
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
T Vector<T, N>::angleBetweenNormalized(Vector const& v) const {
|
||||
return acos(*this * v);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
T Vector<T, N>::magnitudeSquared() const {
|
||||
T m = 0;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
m += square((*this)[i]);
|
||||
return m;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
T Vector<T, N>::magnitude() const {
|
||||
return sqrt(magnitudeSquared());
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
void Vector<T, N>::normalize() {
|
||||
T m = magnitude();
|
||||
if (m != 0)
|
||||
*this = (*this) / m;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::normalized() const {
|
||||
T m = magnitude();
|
||||
if (m != 0)
|
||||
return (*this) / m;
|
||||
else
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::projectOnto(Vector const& v) const {
|
||||
T m = v.magnitudeSquared();
|
||||
if (m != 0)
|
||||
return projectOntoNormalized(v) / m;
|
||||
else
|
||||
return Vector();
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::projectOntoNormalized(Vector const& v) const {
|
||||
return ((*this) * v) * v;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::operator-() const {
|
||||
auto v = *this;
|
||||
v.negate();
|
||||
return v;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
void Vector<T, N>::negate() {
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
(*this)[i] = -(*this)[i];
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::abs() const {
|
||||
Vector v;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
v[i] = fabs((*this)[i]);
|
||||
return v;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::floor() const {
|
||||
return floor(*this);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::ceil() const {
|
||||
return ceil(*this);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::round() const {
|
||||
return round(*this);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
void Vector<T, N>::reverse() {
|
||||
std::reverse(Base::begin(), Base::end());
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
void Vector<T, N>::fill(T const& v) {
|
||||
Base::fill(v);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
void Vector<T, N>::clamp(T const& min, T const& max) {
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
(*this)[i] = Star::max(min, Star::min(max, (*this)[i]));
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <typename Function>
|
||||
void Vector<T, N>::transform(Function&& function) {
|
||||
for (auto& e : *this)
|
||||
e = function(e);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <typename Function>
|
||||
Vector<decltype(std::declval<Function>()(std::declval<T>())), N> Vector<T, N>::transformed(Function&& function) const {
|
||||
return Star::transform<Vector<decltype(std::declval<Function>()(std::declval<T>())), N>>(*this, function);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::operator+(Vector const& v) const {
|
||||
Vector r;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
r[i] = (*this)[i] + v[i];
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::operator-(Vector const& v) const {
|
||||
Vector r;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
r[i] = (*this)[i] - v[i];
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
T Vector<T, N>::operator*(Vector const& v) const {
|
||||
T sum = 0;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
sum += (*this)[i] * v[i];
|
||||
return sum;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::operator*(T s) const {
|
||||
Vector r;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
r[i] = (*this)[i] * s;
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> Vector<T, N>::operator/(T s) const {
|
||||
Vector r;
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
r[i] = (*this)[i] / s;
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N>& Vector<T, N>::operator+=(Vector const& v) {
|
||||
return (*this = *this + v);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N>& Vector<T, N>::operator-=(Vector const& v) {
|
||||
return (*this = *this - v);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N>& Vector<T, N>::operator*=(T s) {
|
||||
return (*this = *this * s);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N>& Vector<T, N>::operator/=(T s) {
|
||||
return (*this = *this / s);
|
||||
}
|
||||
|
||||
// Vector2
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::withAngle(T angle, T magnitude) -> Enable2D<P, Vector<T, N>> {
|
||||
return Vector(std::cos(angle) * magnitude, std::sin(angle) * magnitude);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::angleBetween2(Vector const& v1, Vector const& v2) -> Enable2D<P, T> {
|
||||
// TODO: Inefficient
|
||||
return v2.angle() - v1.angle();
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::angleFormedBy2(Vector const& a, Vector const& b, Vector const& c) -> Enable2D<P, T> {
|
||||
return angleBetween2(b - a, b - c);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::angleFormedBy2(
|
||||
Vector const& a, Vector const& b, Vector const& c, std::function<Vector(Vector, Vector)> const& diff)
|
||||
-> Enable2D<P, T> {
|
||||
return angleBetween2(diff(b, a), diff(b, c));
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::angle() const -> Enable2D<P, T> {
|
||||
return atan2(Base::operator[](1), Base::operator[](0));
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::rotate(T a) const -> Enable2D<P, Vector<T, N>> {
|
||||
// TODO: Need a Matrix2
|
||||
T cosa = std::cos(a);
|
||||
T sina = std::sin(a);
|
||||
return Vector(
|
||||
Base::operator[](0) * cosa - Base::operator[](1) * sina, Base::operator[](0) * sina + Base::operator[](1) * cosa);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::rot90() const -> Enable2D<P, Vector<T, N>> {
|
||||
return Vector(-y(), x());
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::toPolar() const -> Enable2D<P, Vector<T, N>> {
|
||||
return Vector(angle(), Base::magnitude());
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::toCartesian() const -> Enable2D<P, Vector<T, N>> {
|
||||
return vec2d(sin((*this)[0]) * (*this)[1], cos((*this)[0]) * (*this)[1]);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::x() const -> Enable2DOrHigher<P, T> const & {
|
||||
return Base::operator[](0);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::y() const -> Enable2DOrHigher<P, T> const & {
|
||||
return Base::operator[](1);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::setX(T const& t) -> Enable2DOrHigher<P> {
|
||||
Base::operator[](0) = t;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::setY(T const& t) -> Enable2DOrHigher<P> {
|
||||
Base::operator[](1) = t;
|
||||
}
|
||||
|
||||
// Vector3
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::tripleScalarProduct(Vector const& a, Vector const& b, Vector const& c) -> Enable3D<P, T> {
|
||||
return a * (b ^ c);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::theta() const -> Enable3D<P, T> {
|
||||
Vector<T, N> vn = norm(*this);
|
||||
T tmp = fabs(vn.z());
|
||||
if (tmp > 0.99999) {
|
||||
return tmp > 0.0 ? T(-Constants::pi / 2) : T(Constants::pi / 2);
|
||||
} else {
|
||||
return asin(-vn.z());
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::psi() const -> Enable3D<P, T> {
|
||||
Vector<T, N> vn = norm(*this);
|
||||
T tmp = T(fabs(vn.z()));
|
||||
if (tmp > 0.99999) {
|
||||
return 0.0;
|
||||
} else {
|
||||
return T(atan2(vn.y(), vn.x()));
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::thetaEnu() const -> Enable3D<P, T> {
|
||||
Vector<T, N> vn = norm(*this);
|
||||
T tmp = fabs(vn.z());
|
||||
if (tmp > 0.99999) {
|
||||
return tmp > 0.0 ? -Constants::pi / 2 : Constants::pi / 2;
|
||||
} else {
|
||||
return asin(vn.z());
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::psiEnu() const -> Enable3D<P, T> {
|
||||
Vector<T, N> vn = norm(*this);
|
||||
T tmp = fabs(vn.z());
|
||||
if (tmp > 0.99999) {
|
||||
return 0.0;
|
||||
} else {
|
||||
return atan2(vn.x(), vn.y());
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::eulers() const -> Enable3D<P, Vector<T, 2>> {
|
||||
T psi, theta;
|
||||
Vector<T, N> vn = norm(*this);
|
||||
T tmp = fabs(vn.z());
|
||||
if (tmp > 0.99999) {
|
||||
psi = 0.0;
|
||||
theta = tmp > 0.0 ? -Constants::pi / 2 : Constants::pi / 2;
|
||||
} else {
|
||||
psi = atan2(vn.y(), vn.x());
|
||||
theta = asin(-vn.z());
|
||||
}
|
||||
return Vector<T, 2>(psi, theta);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::fromAngles(T psi, T theta) -> Enable3D<P, Vector<T, N>> {
|
||||
Vec3F nv;
|
||||
T cosTheta = T(cos(theta));
|
||||
|
||||
nv.x() = T(cos(psi));
|
||||
nv.y() = T(sin(psi));
|
||||
nv.x() *= cosTheta;
|
||||
nv.y() *= cosTheta;
|
||||
nv.z() = T(-sin(theta));
|
||||
return nv;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::fromAnglesEnu(T psi, T theta) -> Enable3D<P, Vector<T, N>> {
|
||||
Vector nv = fromAngles(psi, theta);
|
||||
return Vector(nv.y(), nv.x(), -nv.z());
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::angle(Vector const& v1, Vector const& v2) -> Enable3D<P, T> {
|
||||
return acos(Star::min(norm(v1) * norm(v2), 1.0));
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::nedToEnu() const -> Enable3D<P, Vector<T, N>> {
|
||||
return Vector(y(), x(), -z());
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::enuToNed() const -> Enable3D<P, Vector<T, N>> {
|
||||
return Vector(y(), x(), -z());
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::z() const -> Enable3DOrHigher<P, T> const & {
|
||||
return Base::operator[](2);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::setZ(T const& t) -> Enable3DOrHigher<P> {
|
||||
Base::operator[](2) = t;
|
||||
}
|
||||
|
||||
// Vector4
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::w() const -> Enable4DOrHigher<P, T> const & {
|
||||
return Base::operator[](3);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
template <size_t P>
|
||||
auto Vector<T, N>::setW(T const& t) -> Enable4DOrHigher<P> {
|
||||
Base::operator[](3) = t;
|
||||
}
|
||||
|
||||
// Free Functions
|
||||
|
||||
template <typename T, size_t N>
|
||||
std::ostream& operator<<(std::ostream& os, Vector<T, N> const& v) {
|
||||
os << '(';
|
||||
for (size_t i = 0; i < N; ++i) {
|
||||
os << v[i];
|
||||
if (i != N - 1)
|
||||
os << ", ";
|
||||
}
|
||||
os << ')';
|
||||
return os;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> operator*(T s, Vector<T, N> v) {
|
||||
return v * s;
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> vnorm(Vector<T, N> v) {
|
||||
return v.normalized();
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
T vmag(Vector<T, N> const& v) {
|
||||
return v.magnitude();
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
T vmagSquared(Vector<T, N> const& v) {
|
||||
return v.magnitudeSquared();
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> vmin(Vector<T, N> const& a, Vector<T, N> const& b) {
|
||||
return a.piecewiseMin(b);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> vmax(Vector<T, N> const& a, Vector<T, N> const& b) {
|
||||
return a.piecewiseMax(b);
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N> vclamp(Vector<T, N> const& a, Vector<T, N> const& min, Vector<T, N> const& max) {
|
||||
return a.piecewiseClamp(min, max);
|
||||
}
|
||||
|
||||
template <typename VectorType>
|
||||
VectorType vmult(VectorType const& a, VectorType const& b) {
|
||||
return a.piecewiseMultiply(b);
|
||||
}
|
||||
|
||||
template <typename VectorType>
|
||||
VectorType vdiv(VectorType const& a, VectorType const& b) {
|
||||
return a.piecewiseDivide(b);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
Vector<T, 3> operator^(Vector<T, 3> v1, Vector<T, 3> v2) {
|
||||
return Vector<T, 3>(v1[1] * v2[2] - v1[2] * v2[1], v1[2] * v2[0] - v1[0] * v2[2], v1[0] * v2[1] - v1[1] * v2[0]);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T operator^(Vector<T, 2> const& v1, Vector<T, 2> const& v2) {
|
||||
return v1[0] * v2[1] - v1[1] * v2[0];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
Loading…
Add table
Add a link
Reference in a new issue