pip/libs/main/math/pimathvector.h

/*! \file pimathvector.h
 * \ingroup Math
 * \~\brief
 * \~english Math vector
 * \~russian Математический вектор
 */
/*
	PIP - Platform Independent Primitives
	PIMathVector
	Ivan Pelipenko peri4ko@yandex.ru, Andrey Bychkov work.a.b@yandex.ru

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU Lesser General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef PIMATHVECTOR_H
#define PIMATHVECTOR_H

#include "pimathbase.h"


template<uint Cols, uint Rows, typename Type>
class PIMathMatrixT;

#define PIMATHVECTOR_ZERO_CMP Type(1E-100)


/// Vector templated

#define PIMV_FOR for (uint i = 0; i < Size; ++i)

template<uint Size, typename Type = double>
class PIP_EXPORT PIMathVectorT {
	typedef PIMathVectorT<Size, Type> _CVector;
	static_assert(std::is_arithmetic<Type>::value, "Type must be arithmetic");
	static_assert(Size > 0, "Size must be > 0");
public:
	PIMathVectorT(const Type & v = Type()) {PIMV_FOR c[i] = v;}
	PIMathVectorT(const PIVector<Type> & val) {
		assert(Size == val.size());
		PIMV_FOR c[i] = val[i];
	}
	PIMathVectorT(std::initializer_list<Type> init_list) {
		assert(Size == init_list.size());
		PIMV_FOR c[i] = init_list.begin()[i];
	}
	static _CVector fromTwoPoints(const _CVector & st, const _CVector & fn) {
		_CVector tv;
		PIMV_FOR tv[i] = fn[i] - st[i];
		return tv;
	}

	constexpr uint size() const {return Size;}
	_CVector & fill(const Type & v) {PIMV_FOR c[i] = v; return *this;}
	_CVector & move(const Type & v) {PIMV_FOR c[i] += v; return *this;}
	_CVector & move(const _CVector & v) {PIMV_FOR c[i] += v[i]; return *this;}
	_CVector & swapElements(uint f, uint s) {
		piSwap<Type>(c[f], c[s]);
		return *this;
	}
	Type lengthSqr() const {
		Type tv(0);
		PIMV_FOR tv += c[i] * c[i];
		return tv;
	}
	Type length() const {return std::sqrt(lengthSqr());}
	Type manhattanLength() const {
		Type tv(0);
		PIMV_FOR tv += piAbs<Type>(c[i]);
		return tv;
	}
	Type angleCos(const _CVector & v) const {
		Type tv = v.length() * length();
		assert(piAbs<Type>(tv) > PIMATHVECTOR_ZERO_CMP);
		return dot(v) / tv;
	}
	Type angleSin(const _CVector & v) const {
		Type tv = angleCos(v);
		return std::sqrt(Type(1) - tv * tv);
	}
	Type angleRad(const _CVector & v) const {return std::acos(angleCos(v));}
	Type angleDeg(const _CVector & v) const {return toDeg(angleRad(v));}
	Type angleElevation(const _CVector & v) const {return 90.0 - angleDeg(v - *this);}
	_CVector projection(const _CVector & v) {
		Type tv = v.length();
		assert(piAbs<Type>(tv) > PIMATHVECTOR_ZERO_CMP);
		return v * (dot(v) / tv);
	}
	_CVector & normalize() {
		Type tv = length();
		assert(piAbs<Type>(tv) > PIMATHVECTOR_ZERO_CMP);
		if (tv == Type(1)) return *this;
		PIMV_FOR c[i] /= tv;
		return *this;
	}
	_CVector normalized() {_CVector tv(*this); tv.normalize(); return tv;}
	bool isNull() const {PIMV_FOR if (c[i] != Type(0)) return false; return true;}
	bool isOrtho(const _CVector & v) const {return ((*this) ^ v) == Type(0);}

	Type & operator [](uint index) {return c[index];}
	const Type & operator [](uint index) const {return c[index];}
	Type at(uint index) const {return c[index];}

	_CVector & operator =(const Type & v) {PIMV_FOR c[i] = v; return *this;}

	bool operator ==(const _CVector & v) const {PIMV_FOR if (c[i] != v[i]) return false; return true;}
	bool operator !=(const _CVector & v) const {return !(*this == c);}

	void operator +=(const _CVector & v) {PIMV_FOR c[i] += v[i];}
	void operator -=(const _CVector & v) {PIMV_FOR c[i] -= v[i];}
	void operator *=(const Type & v) {PIMV_FOR c[i] *= v;}
	void operator /=(const Type & v) {
		assert(piAbs<Type>(v) > PIMATHVECTOR_ZERO_CMP);
		PIMV_FOR c[i] /= v;
	}
	_CVector operator -() const {
		_CVector tv;
		PIMV_FOR tv[i] = -c[i];
		return tv;
	}
	_CVector operator +(const _CVector & v) const {
		_CVector tv(*this);
		PIMV_FOR tv[i] += v[i];
		return tv;
	}
	_CVector operator -(const _CVector & v) const {
		_CVector tv(*this);
		PIMV_FOR tv[i] -= v[i];
		return tv;
	}
	_CVector operator *(const Type & v) const {
		_CVector tv(*this);
		PIMV_FOR tv[i] *= v;
		return tv;
	}
	_CVector operator /(const Type & v) const {
		assert(piAbs<Type>(v) > PIMATHVECTOR_ZERO_CMP);
		_CVector tv = _CVector(*this);
		PIMV_FOR tv[i] /= v;
		return tv;
	}

	_CVector cross(const _CVector & v) const {
		static_assert(Size == 3, "cross product avalible only for 3D vectors");
		_CVector tv;
		tv[0] = c[1]*v[2] - v[1]*c[2];
		tv[1] = v[0]*c[2] - c[0]*v[2];
		tv[2] = c[0]*v[1] - v[0]*c[1];
		return tv;
	}
	Type dot(const _CVector & v) const {
		Type tv(0);
		PIMV_FOR tv += c[i] * v[i];
		return tv;
	}
	_CVector mul(const _CVector & v) const {
		_CVector tv(*this);
		PIMV_FOR tv[i] *= v[i];
		return tv;
	}
	_CVector mul(const Type & v) const {
		return (*this) * v;
	}
	_CVector div(const _CVector & v) const {
		_CVector tv(*this);
		PIMV_FOR {
			assert(piAbs<Type>(v[i]) > PIMATHVECTOR_ZERO_CMP);
			tv[i] /= v[i];
		}
		return tv;
	}
	_CVector div(const Type & v) const {
		return (*this) / v;
	}

	PIMathMatrixT<1, Size, Type> transposed() const {
		PIMathMatrixT<1, Size, Type> ret;
		PIMV_FOR ret[0][i] = c[i];
		return ret;
	}

	Type distToLine(const _CVector & lp0, const _CVector & lp1) {
		_CVector a(lp0, lp1);
		Type tv = a.length();
		assert(piAbs<Type>(tv) > PIMATHVECTOR_ZERO_CMP);
		_CVector b(lp0, *this);
		return piAbs<Type>(a[0]*b[1] - a[1]*b[0]) / tv;
	}

	template<uint Size1, typename Type1> /// vector {Size, Type} to vector {Size1, Type1}
	PIMathVectorT<Size1, Type1> turnTo() const {
		PIMathVectorT<Size1, Type1> tv;
		uint sz = piMin<uint>(Size, Size1);
		for (uint i = 0; i < sz; ++i) tv[i] = c[i];
		return tv;
	}

	static _CVector cross(const _CVector & v1, const _CVector & v2) {
		return v1.cross(v2);
	}
	static _CVector dot(const _CVector & v1, const _CVector & v2) {
		return v1.dot(v2);
	}
	static _CVector mul(const _CVector & v1, const _CVector & v2) {
		return v1.mul(v2);
	}
	static _CVector mul(const Type & v1, const _CVector & v2) {
		return v2 * v1;
	}
	static _CVector mul(const _CVector & v1, const Type & v2) {
		return v1 * v2;
	}
	static _CVector div(const _CVector & v1, const _CVector & v2) {
		return v1.div(v2);
	}
	static _CVector div(const _CVector & v1, const Type & v2) {
		return v1 / v2;
	}

private:
	Type c[Size];

};

template<uint Size, typename Type>
inline PIMathVectorT<Size, Type> operator *(const Type & x, const PIMathVectorT<Size, Type> & v) {
	return v * x;
}

template<uint Size, typename Type>
inline PICout operator <<(PICout s, const PIMathVectorT<Size, Type> & v) {s << "{"; PIMV_FOR {s << v[i]; if (i < Size - 1) s << ", ";} s << "}"; return s;}


typedef PIMathVectorT<2u, int> PIMathVectorT2i;
typedef PIMathVectorT<3u, int> PIMathVectorT3i;
typedef PIMathVectorT<4u, int> PIMathVectorT4i;
typedef PIMathVectorT<2u, double> PIMathVectorT2d;
typedef PIMathVectorT<3u, double> PIMathVectorT3d;
typedef PIMathVectorT<4u, double> PIMathVectorT4d;


#undef PIMV_FOR

/// Vector

#define PIMV_FOR for (uint i = 0; i < c.size(); ++i)

template<typename Type>
class PIP_EXPORT PIMathVector {
	typedef PIMathVector<Type> _CVector;
	template <typename P, typename Type1>
	friend PIBinaryStream<P> & operator <<(PIBinaryStream<P> & s, const PIMathVector<Type1> & v);
	template <typename P, typename Type1>
	friend PIBinaryStream<P> & operator >>(PIBinaryStream<P> & s, PIMathVector<Type1> & v);
public:
	PIMathVector(const uint size = 0, const Type & new_value = Type()) {c.resize(size, new_value);}
	PIMathVector(const PIVector<Type> & val) {c = val;}
	PIMathVector(PIVector<Type> && val) : c(std::move(val)) {}
	PIMathVector(std::initializer_list<Type> init_list) {c = PIVector<Type>(init_list);}

	template<uint Size>
	PIMathVector(const PIMathVectorT<Size, Type> & val) {c.resize(Size); PIMV_FOR c[i] = val[i];}

	static PIMathVector fromTwoPoints(const _CVector & st, const _CVector & fn) {
		assert(st.size() == fn.size());
		_CVector v(st.size());
		for (uint i = 0; i < v.size(); ++i) v.c[i] = fn[i] - st[i];
	}

	static PIMathVector zeros(const uint size) {return PIMathVector(size, Type());}
	static PIMathVector ones(const uint size) {return PIMathVector(size, Type(1));}
	static PIMathVector arange(const Type start, const Type stop, const Type step = Type(1)) {
		PIVector<Type> v;
		for (Type i = start; i < stop; i+= step) v << i;
		return PIMathVector(std::move(v));
	}

	uint size() const {return c.size();}
	_CVector & resize(uint size, const Type & new_value = Type()) {
		c.resize(size, new_value);
		return *this;
	}
	_CVector resized(uint size, const Type & new_value = Type()) {
		_CVector tv = _CVector(*this);
		tv.resize(size, new_value);
		return tv;
	}
	_CVector & fill(const Type & v) {
		c.fill(v);
		return *this;
	}
	_CVector & move(const Type & v) {
		PIMV_FOR c[i] += v;
		return *this;
	}
	_CVector & move(const _CVector & v) {
		assert(c.size() == v.size());
		PIMV_FOR c[i] += v[i];
		return *this;
	}
	_CVector & swapElements(uint f, uint s) {
		piSwap<Type>(c[f], c[s]);
		return *this;
	}
	Type lengthSqr() const {
		Type tv(0);
		PIMV_FOR tv += c[i] * c[i];
		return tv;
	}
	Type length() const {return std::sqrt(lengthSqr());}
	Type manhattanLength() const {
		Type tv(0);
		PIMV_FOR tv += piAbs<Type>(c[i]);
		return tv;
	}
	Type angleCos(const _CVector & v) const {
		assert(c.size() == v.size());
		Type tv = v.length() * length();
		assert(piAbs<Type>(tv) > PIMATHVECTOR_ZERO_CMP);
		return dot(v) / tv;
	}
	Type angleSin(const _CVector & v) const {
		assert(c.size() == v.size());
		Type tv = angleCos(v);
		return std::sqrt(Type(1) - tv * tv);
	}
	Type angleRad(const _CVector & v) const {return std::acos(angleCos(v));}
	Type angleDeg(const _CVector & v) const {return toDeg(angleRad(v));}
	_CVector projection(const _CVector & v) {
		assert(c.size() == v.size());
		Type tv = v.length();
		assert(piAbs<Type>(tv) > PIMATHVECTOR_ZERO_CMP);
		return v * (dot(v) / tv);
	}
	_CVector & normalize() {
		Type tv = length();
		assert(piAbs<Type>(tv) > PIMATHVECTOR_ZERO_CMP);
		if (tv == Type(1)) return *this;
		PIMV_FOR c[i] /= tv;
		return *this;
	}
	_CVector normalized() {
		_CVector tv(*this);
		tv.normalize();
		return tv;
	}
	bool isNull() const {
		PIMV_FOR if (c[i] != Type(0)) return false;
		return true;
	}
	bool isValid() const {return !c.isEmpty();}
	bool isOrtho(const _CVector & v) const {return dot(v) == Type(0);}

	Type & operator [](uint index) {return c[index];}
	const Type & operator [](uint index) const {return c[index];}
	Type at(uint index) const {return c[index];}

	_CVector & operator =(const Type & v) {PIMV_FOR c[i] = v; return *this;}

	bool operator ==(const _CVector & v) const {return c == v.c;}
	bool operator !=(const _CVector & v) const {return c != v.c;}

	void operator +=(const _CVector & v) {
		assert(c.size() == v.size());
		PIMV_FOR c[i] += v[i];
	}
	void operator -=(const _CVector & v) {
		assert(c.size() == v.size());
		PIMV_FOR c[i] -= v[i];
	}
	void operator *=(const Type & v) {PIMV_FOR c[i] *= v;}
	void operator /=(const Type & v) {
		assert(piAbs<Type>(v) > PIMATHVECTOR_ZERO_CMP);
		PIMV_FOR c[i] /= v;
	}
	_CVector operator -() const {
		_CVector tv(c.size());
		PIMV_FOR tv[i] = -c[i];
		return tv;
	}
	_CVector operator +(const _CVector & v) const {
		assert(c.size() == v.size());
		_CVector tv(*this);
		PIMV_FOR tv[i] += v[i];
		return tv;
	}
	_CVector operator -(const _CVector & v) const {
		assert(c.size() == v.size());
		_CVector tv(*this);
		PIMV_FOR tv[i] -= v[i];
		return tv;
	}
	_CVector operator *(const Type & v) const {
		_CVector tv(*this);
		PIMV_FOR tv[i] *= v;
		return tv;
	}
	_CVector operator /(const Type & v) const {
		assert(piAbs<Type>(v) > PIMATHVECTOR_ZERO_CMP);
		_CVector tv(*this);
		PIMV_FOR tv[i] /= v;
		return tv;
	}
	_CVector cross(const _CVector & v) const {
		assert(c.size() == 3);
		assert(v.size() == 3);
		_CVector tv(3);
		tv[0] = c[1]*v[2] - v[1]*c[2];
		tv[1] = c[2]*v[0] - v[2]*c[0];
		tv[2] = c[0]*v[1] - v[0]*c[1];
		return tv;
	}
	Type dot(const _CVector & v) const {
		assert(c.size() == v.size());
		Type tv(0);
		PIMV_FOR tv += c[i] * v[i];
		return tv;
	}
	_CVector mul(const _CVector & v) const {
		assert(c.size() == v.size());
		_CVector tv(*this);
		PIMV_FOR tv[i] *= v[i];
		return tv;
	}
	_CVector mul(const Type & v) const {
		return (*this) * v;
	}
	_CVector div(const _CVector & v) const {
		assert(c.size() == v.size());
		_CVector tv(*this);
		PIMV_FOR {
			assert(piAbs<Type>(v[i]) > PIMATHVECTOR_ZERO_CMP);
			tv[i] /= v[i];
		}
		return tv;
	}
	_CVector div(const Type & v) const {
		return (*this) / v;
	}

	Type distToLine(const _CVector & lp0, const _CVector & lp1) {
		assert(c.size() == lp0.size());
		assert(c.size() == lp1.size());
		_CVector a = _CVector::fromTwoPoints(lp0, lp1);
		Type tv = a.length();
		assert(piAbs<Type>(tv) > PIMATHVECTOR_ZERO_CMP);
		_CVector b = _CVector::fromTwoPoints(lp0, *this);
		return piAbs<Type>(a[0]*b[1] - a[1]*b[0]) / tv;
	}

	PIVector<Type> toVector() const {return c;}

	void forEach(std::function<void(const Type &)> f) const {
		c.forEach(f);
	}
	_CVector & forEach(std::function<void(Type &)> f) {
		c.forEach(f);
		return *this;
	}

	inline Type * data() {return c.data();}
	inline const Type * data() const {return c.data();}


	static _CVector cross(const _CVector & v1, const _CVector & v2) {
		return v1.cross(v2);
	}
	static _CVector dot(const _CVector & v1, const _CVector & v2) {
		return v1.dot(v2);
	}
	static _CVector mul(const _CVector & v1, const _CVector & v2) {
		return v1.mul(v2);
	}
	static _CVector mul(const Type & v1, const _CVector & v2) {
		return v2 * v1;
	}
	static _CVector mul(const _CVector & v1, const Type & v2) {
		return v1 * v2;
	}
	static _CVector div(const _CVector & v1, const _CVector & v2) {
		return v1.div(v2);
	}
	static _CVector div(const _CVector & v1, const Type & v2) {
		return v1 / v2;
	}
private:
	PIVector<Type> c;
};

template<typename Type>
inline PIMathVector<Type> operator *(const Type & x, const PIMathVector<Type> & v) {
	return v * x;
}

#undef PIMV_FOR

#ifdef PIP_STD_IOSTREAM
template<typename Type>
inline std::ostream & operator <<(std::ostream & s, const PIMathVector<Type> & v) {s << "{"; for (uint i = 0; i < v.size(); ++i) {s << v[i]; if (i < v.size() - 1) s << ", ";} s << "}"; return s;}
#endif

template<typename Type>
inline PICout operator <<(PICout s, const PIMathVector<Type> & v) {s << "Vector{"; for (uint i = 0; i < v.size(); ++i) {s << v[i]; if (i < v.size() - 1) s << ", ";} s << "}"; return s;}

template <typename P, typename T>
inline PIBinaryStream<P> & operator <<(PIBinaryStream<P> & s, const PIMathVector<T> & v) {s << v.c; return s;}
template <typename P, typename T>
inline PIBinaryStream<P> & operator >>(PIBinaryStream<P> & s, PIMathVector<T> & v) {s >> v.c; return s;}


typedef PIMathVector<int> PIMathVectori;
typedef PIMathVector<double> PIMathVectord;

#endif // PIMATHVECTOR_H