181 lines
2.8 KiB
C++
181 lines
2.8 KiB
C++
#ifndef __WRNAV_GEOM_VECTOR_H
|
|
#define __WRNAV_GEOM_VECTOR_H
|
|
|
|
|
|
#include <array>
|
|
#include <cassert>
|
|
#include <cmath>
|
|
#include <initializer_list>
|
|
#include <ostream>
|
|
#include <iostream>
|
|
|
|
#include <wrnav/util/math.h>
|
|
|
|
|
|
namespace wr {
|
|
namespace geom {
|
|
|
|
template <typename T, size_t N>
|
|
class Vector {
|
|
public:
|
|
Vector() { wr::util::DefaultEpsilon(this->epsilon); }
|
|
|
|
Vector(std::initializer_list<T> ilst)
|
|
{
|
|
assert(ilst.size() == N);
|
|
|
|
wr::util::DefaultEpsilon(this->epsilon);
|
|
std::copy(ilst.begin(), ilst.end(), this->arr.begin());
|
|
}
|
|
|
|
|
|
T magnitude() const {
|
|
T result = 0;
|
|
|
|
for (size_t i = 0; i < N; i++) {
|
|
result += (this->arr[i] * this->arr[i]);
|
|
}
|
|
return std::sqrt(result);
|
|
};
|
|
|
|
|
|
void
|
|
setEpsilon(T epsilon)
|
|
{
|
|
this->epsilon = epsilon;
|
|
}
|
|
|
|
|
|
bool
|
|
isZero() const
|
|
{
|
|
for (size_t i = 0; i < N; i++) {
|
|
if (!wr::util::WithinTolerance(this->arr[i], 0.0, this->epsilon)) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
Vector
|
|
unitVector() const
|
|
{
|
|
return *this / this->magnitude();
|
|
}
|
|
|
|
|
|
T
|
|
angle(const Vector<T, N> &rhs) const
|
|
{
|
|
Vector<T, N> unitA = this->unitVector();
|
|
Vector<T, N> unitB = rhs.unitVector();
|
|
|
|
return std::acos(unitA * unitB);
|
|
}
|
|
|
|
|
|
Vector operator+(const Vector<T, N> &rhs) const {
|
|
Vector<T, N> vec;
|
|
|
|
for (size_t i = 0; i < N; i++) {
|
|
vec.arr[i] = this->arr[i] + rhs.arr[i];
|
|
}
|
|
|
|
return vec;
|
|
}
|
|
|
|
|
|
Vector operator-(const Vector<T, N> &rhs) const {
|
|
Vector<T, N> vec;
|
|
|
|
for (size_t i = 0; i < N; i++) {
|
|
vec.arr[i] = this->arr[i] - rhs.arr[i];
|
|
}
|
|
|
|
return vec;
|
|
}
|
|
|
|
|
|
// Scalar multiplication.
|
|
Vector operator*(const T k) const {
|
|
Vector<T, N> vec;
|
|
|
|
for (size_t i = 0; i < N; i++) {
|
|
vec.arr[i] = this->arr[i] * k;
|
|
}
|
|
|
|
return vec;
|
|
}
|
|
|
|
|
|
// Scalar division.
|
|
Vector operator/(const T k) const {
|
|
Vector<T, N> vec;
|
|
|
|
for (size_t i = 0; i < N; i++) {
|
|
vec.arr[i] = this->arr[i] / k;
|
|
}
|
|
|
|
return vec;
|
|
}
|
|
|
|
|
|
// Dot product.
|
|
T operator*(const Vector<T, N> &rhs) const {
|
|
T result = 0;
|
|
|
|
for (size_t i = 0; i < N; i++) {
|
|
result += (this->arr[i] * rhs.arr[i]);
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
|
|
bool operator==(const Vector<T, N> &rhs) const {
|
|
for (size_t i = 0; i<N; i++) {
|
|
if (!wr::util::WithinTolerance(this->arr[i], rhs.arr[i], this->epsilon)) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
bool operator!=(const Vector<T, N> &rhs) const {
|
|
return !(*this == rhs);
|
|
}
|
|
|
|
|
|
friend std::ostream& operator<<(std::ostream& outs, const Vector<T, N>& vec) {
|
|
outs << "<";
|
|
for (size_t i = 0; i < N; i++) {
|
|
outs << vec.arr[i];
|
|
if (i < (N-1)) {
|
|
outs << ", ";
|
|
}
|
|
}
|
|
outs << ">";
|
|
return outs;
|
|
}
|
|
|
|
private:
|
|
static const size_t dim = N;
|
|
T epsilon;
|
|
std::array<T, N> arr;
|
|
};
|
|
|
|
|
|
typedef Vector<float, 3> Vector3f;
|
|
typedef Vector<float, 4> Vector4f;
|
|
typedef Vector<double, 3> Vector3d;
|
|
typedef Vector<double, 4> Vector4d;
|
|
|
|
|
|
} // namespace geom
|
|
} // namespace wr
|
|
|
|
|
|
#endif // __WRNAV_GEOM_VECTOR_H
|