wrmath/wrmath/geom/vector.h

/// vector.h provides an implementation of vectors.
#ifndef __WRMATH_GEOM_VECTOR_H
#define __WRMATH_GEOM_VECTOR_H


#include <array>
#include <cassert>
#include <cmath>
#include <initializer_list>
#include <ostream>
#include <iostream>

#include "wrmath/math.h"


// This implementation is essentially a C++ translation of a Python library
// I wrote for Coursera's "Linear Algebra for Machine Learning" course. Many
// of the test vectors come from quiz questions in the class.


namespace wr {
namespace geom {


/// @brief Vectors represent a direction and magnitude.
///
/// Vector provides a standard interface for dimensionless fixed-size
/// vectors. Once instantiated, they cannot be modified.
///
/// Note that while the class is templated, it's intended to be used with
/// floating-point types.
///
/// Vectors can be indexed like arrays, and they contain an epsilon value
/// that defines a tolerance for equality.
template <typename T, size_t N>
class Vector {
public:
    	/// The default constructor creates a unit vector for a given type
    	/// and size.
	Vector()
	{
		T	unitLength = (T)1.0 / std::sqrt(N);
		for (size_t i = 0; i < N; i++) {
			this->arr[i] = unitLength;
		}

		wr::math::DefaultEpsilon(this->epsilon);
	}


	/// If given an initializer_list, the vector is created with
	/// those values. There must be exactly N elements in the list.
	/// @param ilst An intializer list with N elements of type T.
	Vector(std::initializer_list<T>	ilst)
	{
		assert(ilst.size() == N);

		wr::math::DefaultEpsilon(this->epsilon);
		std::copy(ilst.begin(), ilst.end(), this->arr.begin());
	}

	/// Construct a vector from a std::array with default epsilon.
	explicit Vector(const std::array<T, N>& src)
	{
		wr::math::DefaultEpsilon(this->epsilon);
		this->arr = src;
	}


	/// Compute the length of the vector.
	/// @return The length of the vector.
	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);
	}


	/// Set the tolerance for equality checks. At a minimum, this allows
	/// for systemic errors in floating math arithmetic.
	/// @param eps is the maximum difference between this vector and
	///            another.
	void
	setEpsilon(T eps)
	{
		this->epsilon = eps;
	}


	/// Determine whether this is a zero vector.
	/// @return true if the vector is zero.
	bool
	isZero() const
	{
		for (size_t i = 0; i < N; i++) {
			if (!wr::math::WithinTolerance(this->arr[i], (T)0.0, this->epsilon)) {
				return false;
			}
		}
		return true;
	}


	/// Obtain the unit vector for this vector.
	/// @return The unit vector
	Vector
	unitVector() const
	{
		return *this / this->magnitude();
	}


	/// Determine if this is a unit vector, e.g. if its length is 1.
	/// @return true if the vector is a unit vector.
	bool
	isUnitVector() const
	{
		return wr::math::WithinTolerance(this->magnitude(), (T)1.0, this->epsilon);
	}


	/// Compute the angle between two other vectors.
	/// @param other Another vector.
	/// @return The angle in radians between the two vectors.
	T
	angle(const Vector<T, N> &other) const
	{
		Vector<T, N>	unitA = this->unitVector();
		Vector<T, N>	unitB = other.unitVector();

		// Can't compute angles with a zero vector.
		assert(!this->isZero());
		assert(!other.isZero());
		return std::acos(unitA * unitB);
	}


	/// Determine whether two vectors are parallel.
	/// @param other Another vector
	/// @return True if the two vectors are parallel (same direction).
	bool
	isParallel(const Vector<T, N> &other) const
	{
		if (this->isZero() || other.isZero()) {
			return true;
		}

		// Compare unit vectors directly instead of using angle()/acos(), which
		// produces NaN or ~0.0003 on macOS/arm64 due to dot products landing
		// just above 1.0 (e.g. 1.000000001) — outside the domain of acos.
		return this->unitVector() == other.unitVector();
	}


	/// Determine if two vectors are orthogonal or perpendicular to each
	/// other.
	/// @param other Another vector
	/// @return True if the two vectors are orthogonal.
	bool
	isOrthogonal(const Vector<T, N> &other) const
	{
		if (this->isZero() || other.isZero()) {
			return true;
		}

		return wr::math::WithinTolerance(*this * other, (T)0.0, this->epsilon);
	}


	/// Project this vector onto some basis vector.
	/// @param basis The basis vector to be projected onto.
	/// @return A vector that is the projection of this onto the basis
	///         vector.
	Vector
	projectParallel(const Vector<T, N> &basis) const
	{
		Vector<T, N>	unit_basis = basis.unitVector();

		return unit_basis * (*this * unit_basis);
	}


	/// Project this vector perpendicularly onto some basis vector.
	/// This is also called the rejection of the vector.
	/// @param basis The basis vector to be projected onto.
	/// @return A vector that is the orthogonal projection of this onto
	///         the basis vector.
	Vector
	projectOrthogonal(const Vector<T, N> &basis)
	{
		Vector<T, N>	spar = this->projectParallel(basis);
		return *this - spar;
	}


	/// Compute the cross product of two vectors. This is only defined
	/// over three-dimensional vectors.
	/// @param other Another 3D vector.
	/// @return The cross product vector.
	Vector
	cross(const Vector<T, N> &other) const
	{
		assert(N == 3);
		return Vector<T, N> {
			(this->arr[1] * other.arr[2]) - (other.arr[1] * this->arr[2]),
			-((this->arr[0] * other.arr[2]) - (other.arr[0] * this->arr[2])),
			(this->arr[0] * other.arr[1]) - (other.arr[0] * this->arr[1])
		};
	}


	/// Perform vector addition with another vector.
	/// @param other The vector to be added.
	/// @return A new vector that is the result of adding this and the
	///         other vector.
	Vector
	operator+(const Vector<T, N> &other) const
	{
		Vector<T, N>	vec;

		for (size_t i = 0; i < N; i++) {
			vec.arr[i] = this->arr[i] + other.arr[i];
		}

		return vec;
	}


	/// Perform vector subtraction with another vector.
	/// @param other The vector to be subtracted from this vector.
	/// @return A new vector that is the result of subtracting the
	///         other vector from this one.
	Vector
	operator-(const Vector<T, N> &other) const
	{
		Vector<T, N>	vec;

		for (size_t i = 0; i < N; i++) {
			vec.arr[i] = this->arr[i] - other.arr[i];
		}

		return vec;
	}


	/// Perform scalar multiplication of this vector by some scale factor.
	/// @param k The scaling value.
	/// @return A new vector that is this vector scaled by k.
	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;
	}


	/// Perform scalar division of this vector by some scale factor.
	/// @param k The scaling value
	/// @return A new vector that is this vector scaled by 1/k.
	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;
	}


	/// Compute the dot product between two vectors.
	/// @param other The other vector.
	/// @return A scalar value that is the dot product of the two vectors.
	T
	operator*(const Vector<T, N> &other) const
	{
		T	result = 0;

		for (size_t i = 0; i < N; i++) {
			result += (this->arr[i] * other.arr[i]);
		}

		return result;
	}


	/// Compare two vectors for equality.
	/// @param other The other vector.
	/// @return Return true if all the components of both vectors are
	///         within the tolerance value.
	bool
	operator==(const Vector<T, N> &other) const
	{
		for (size_t i = 0; i<N; i++) {
			if (!wr::math::WithinTolerance(this->arr[i], other.arr[i], this->epsilon)) {
				return false;
			}
		}
		return true;
	}


	/// Compare two vectors for inequality.
	/// @param other The other vector.
	/// @return Return true if any of the components of both vectors are
	///         not within the tolerance value.
	bool
	operator!=(const Vector<T, N> &other) const
	{
		return !(*this == other);
	}


	/// Support array indexing into vector.
	///
	/// Note that the values of the vector cannot be modified. Instead,
	/// it's required to do something like the following:
	///
	/// ```
	/// Vector3d	a {1.0, 2.0, 3.0};
	/// Vector3d	b {a[0], a[1]*2.0, a[2]};
	/// ```
	///
	/// @param i The component index.
	/// @return The value of the vector component at i.
	const T&
	operator[](size_t i) const
	{
		return this->arr[i];
	}


	/// Support outputting vectors in the form "<i, j, ...>".
	/// @param outs An output stream.
	/// @param vec The vector to be formatted.
	/// @return The output stream.
	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;
	}

	/// Return a zero vector with default epsilon.
	static Vector
	zero()
	{
		Vector v;
		for (size_t i = 0; i < N; i++) v.arr[i] = (T)0.0;
		return v;
	}

	/// Return a copy with a different epsilon.
	Vector
	withEpsilon(T eps) const
	{
		Vector v = *this;
		v.epsilon = eps;
		return v;
	}

	/// Access internal array (read-only).
	const std::array<T, N>&
	asArray() const
	{
		return this->arr;
	}

	/// Construct from array with default epsilon.
	static Vector
	fromArray(const std::array<T, N>& src)
	{
		Vector v;
		v.arr = src;
		return v;
	}

	/// Construct from array with custom epsilon.
	static Vector
	fromArrayEps(const std::array<T, N>& src, T eps)
	{
		Vector v;
		v.arr = src;
		v.epsilon = eps;
		return v;
	}

	/// Apply a function to each element and return the result.
	template <typename F>
	Vector
	map(F f) const
	{
		Vector v;
		v.epsilon = this->epsilon;
		for (size_t i = 0; i < N; i++) {
			v.arr[i] = f(this->arr[i]);
		}
		return v;
	}

	/// Return true if all elements are NaN.
	bool
	isNaN() const
	{
		for (size_t i = 0; i < N; i++) {
			if (!std::isnan(this->arr[i])) return false;
		}
		return true;
	}

	/// Compute the signed angle between two vectors in [-pi, pi].
	/// Uses the 2D cross product of the first two components.
	T
	angle2(const Vector<T, N> &other) const
	{
		T cross = this->arr[0] * other.arr[1] - this->arr[1] * other.arr[0];
		T dot   = *this * other;
		return std::atan2(cross, dot);
	}

	/// Compute the Euclidean distance between two vectors.
	T
	euclidist(const Vector<T, N> &other) const
	{
		T result = 0;
		for (size_t i = 0; i < N; i++) {
			T diff = this->arr[i] - other.arr[i];
			result += diff * diff;
		}
		return std::sqrt(result);
	}

	/// Project this vector into a lower-dimensional space by taking
	/// the first M elements.
	template <size_t M>
	Vector<T, M>
	projectLower() const
	{
		static_assert(M <= N, "cannot project to a higher dimension");
		std::array<T, M> result_arr;
		for (size_t i = 0; i < M; i++) {
			result_arr[i] = this->arr[i];
		}
		return Vector<T, M>::fromArrayEps(result_arr, this->epsilon);
	}

	/// Project this vector into a lower-dimensional space by taking
	/// the last M elements.
	template <size_t M>
	Vector<T, M>
	projectLowerTail() const
	{
		static_assert(M <= N, "cannot project to a higher dimension");
		std::array<T, M> result_arr;
		for (size_t i = 0; i < M; i++) {
			result_arr[i] = this->arr[N - M + i];
		}
		return Vector<T, M>::fromArrayEps(result_arr, this->epsilon);
	}

	/// Return the x component (index 0).
	T x() const { static_assert(N >= 1, "x() requires N >= 1"); return this->arr[0]; }

	/// Return the y component (index 1).
	T y() const { static_assert(N >= 2, "y() requires N >= 2"); return this->arr[1]; }

	/// Return the z component (index 2).
	T z() const { static_assert(N >= 3, "z() requires N >= 3"); return this->arr[2]; }

private:
	static const size_t	dim = N;
	T			epsilon;
	std::array<T, N>	arr;
};

///
/// \defgroup vector_aliases Vector type aliases.
///

/// \ingroup vector_aliases
/// A number of shorthand aliases for vectors are provided. They follow
/// the form of VectorNt, where N is the dimension and t is the type.
/// For example, a 2D float vector is Vector2f.

/// \ingroup vector_aliases
/// @brief Type alias for a two-dimensional float vector.
typedef Vector<float,  2>	Vector2f;

/// \ingroup vector_aliases
/// @brief Type alias for a three-dimensional float vector.
typedef Vector<float,  3>	Vector3f;

/// \ingroup vector_aliases
/// @brief Type alias for a four-dimensional float vector.
typedef Vector<float,  4>	Vector4f;

/// \ingroup vector_aliases
/// @brief Type alias for a two-dimensional double vector.
typedef Vector<double,  2>	Vector2d;

/// \ingroup vector_aliases
/// @brief Type alias for a three-dimensional double vector.
typedef	Vector<double, 3>	Vector3d;

/// \ingroup vector_aliases
/// @brief Type alias for a four-dimensional double vector.
typedef Vector<double, 4>	Vector4d;


} // namespace geom
} // namespace wr


#endif //  __WRMATH_GEOM_VECTOR_H