TMP Vector and Square Matrix

sinlge.hpp

/*
 * Copyright 2014 Peter LaValle (with that name as my GMail) / g-pechorin
 *
 * Vector and square matrix header. Header for math functions that work on vectors and square matrices of arbitrary dimensions.
 *
 * You may use this under the Affero GNU GPL http://www.gnu.org/licenses/agpl-3.0.html
 * 
 * Lacks rotation stuff since ... y'know those are specific to specific dimensions of matrix
 * Other than std::string creation (which should only be used for debugging) - all methods are implemented as const templates (no loops or branches) so should optimize nicely
 *
 * ... oh; and this seems to crash VSEE2013 - so I don't know if it actually works right for everything :)
 *
 */
#pragma once

#include <cstdint>
#include <string>
#include <sstream>
#include <cassert>
#include <utility>

typedef float sinlge_t;

// technically - this is the only function in this file; the rest are templates
inline sinlge_t sinlge_sqrt(const sinlge_t input) { return sqrtf(input); }

template<size_t size>
struct sinlge_vec
{
	// this is the "only" data for this sucker
	sinlge_t _data[size];


private: // private implmentation
	/* ====
	* Recursive / man-behind-the-curtain template for scaling
	*/
	template<size_t component>
	void _mul(sinlge_vec<size>& result, const sinlge_t val) const
	{
		result._data[component] = (_data[component] * val);

		_mul<component - 1>(result, val);
	}

	template<>
	void _mul<0>(sinlge_vec<size>& result, const sinlge_t val) const
	{
		result._data[component] = (_data[component] * val);
	}

	/* ====
	* Recursive / man-behind-the-curtain template for dot product
	*/
	template<size_t component>
	sinlge_t _dot(const sinlge_vec<size>& other) const
	{
		return (_data[component - 1] * other._data[component - 1]) + _dot<component - 1>(other);
	}
	template<>
	sinlge_t _dot<0>(const sinlge_vec<size>& other) const
	{
		return 0.f;
	}

	/* ====
	* Recursive / man-behind-the-curtain template for sum (of two vectors)
	*/
	template <size_t component>
	sinlge_vec<size> _sum(const sinlge_vec<size>& other) const
	{
		sinlge_vec<size> result = _sum<component - 1>(other);

		result._data[component - 1] = _data[component - 1] + other._data[component - 1];

		return result;
	}
	template <> sinlge_vec<size> _sum<0>(const sinlge_vec<size>& other) const { return sinlge_vec<size>(); }

	/* ====
	* Recursive / man-behind-the-curtain template for addition (of a scalar)
	*/
	template <size_t component>
	sinlge_vec<size> _add(const sinlge_t val) const
	{
		sinlge_vec<size> result = _add<component - 1>();

		result._data[component - 1] = _data[component - 1] + val;

		return result;
	}
	template <> sinlge_vec<size> _add<0>(const sinlge_t val) const { return sinlge_vec<size>(); }

	/* ====
	* Recursive templates for cross (product)
	*/
	template<size_t component>
	sinlge_vec<size> _crs(const sinlge_vec<size>& other) const
	{
		sinlge_vec<size> result = _crs<component - 1>(other);

		result._data[component - 1] = _data[component % size] * other._data[(component + 1) % size] - _data[(component + 1) % size] * other._data[component  % size];

		return result;
	}
	template <> sinlge_vec<size> _crs<0>(const sinlge_vec<size>& other) const { return sinlge_vec<size>(); }


	/* ====
	* Recursive templates for extension (by adding an element at the end)
	*/
	template<size_t component>
	sinlge_vec<size + 1> _ext(void) const
	{
		sinlge_vec<size + 1> result = _ext<component - 1>();

		result._data[component - 1] = _data[component - 1];

		return result;
	}
	template <> sinlge_vec<size + 1> _ext<0>(void) const { return sinlge_vec<size + 1>(); }

	/* ====
	* Recursive templates for equality
	*/
	template<size_t component>
	bool _equ(const sinlge_vec<size>& other) const
	{
		return (_data[component - 1] == other._data[component - 1]) && _equ<component - 1>(other);
	}
	template <> bool _equ<0>(const sinlge_vec<size>& other) const { return true; }

	/* ====
	* Recursive templates for set
	*/
	template<size_t component>
	static sinlge_vec<size> _set(const sinlge_t vals[])
	{
		sinlge_vec<size> result = _set<component - 1>(vals);

		result._data[component - 1] = vals[component - 1];

		return result;
	}
	template <> static sinlge_vec<size> _set<0>(const sinlge_t vals[]) { return sinlge_vec<size>(); }

	/* ====
	* Recursive templates to drop a row (single element) from this vector
	*/
	template <size_t r>
	sinlge_vec<size - 1> _drop(const size_t row)
	{
		sinlge_vec<size - 1> result = _drop<r - 1>(row);

		if (r != row)
		{
			result._data[(r < row) ? (r - 1) : (r - 2)] = _data[r - 1];
		}

		return result;
	}
	template <> sinlge_vec<size - 1> _drop(const size_t row) { return sinlge_vec<size - 1>(); }

public: // public API
	sinlge_vec<size - 1> drop(const size_t row)
	{
		return _drop<size>(row - 1);
	}

	sinlge_vec<size - 1> tru(void) const
	{
		return drop(size);
	}

	static sinlge_vec<size> arr(const sinlge_t vals[])
	{
		return _set<size>(vals);
	}

	static sinlge_vec<size> var(const std::initializer_list<sinlge_t> vals)
	{
		assert(size == vals.size());
		return _set<size>(vals.begin());
	}

	bool equ(const sinlge_vec<size>& other) const
	{
		return _equ<size>(other);
	}

	sinlge_vec<size + 1> ext(const sinlge_t tail) const
	{
		sinlge_vec<size + 1> result = _ext<size>();

		result._data[size] = tail;

		return result;
	}

	sinlge_vec<size> mul(const sinlge_t other) const
	{
		sinlge_vec<size> result;

		_mul<size - 1>(result, val);

		return result;
	}

	sinlge_vec<size> add(const sinlge_t other) const
	{
		sinlge_vec<size> result;

		_add<size>(result, val);

		return result;
	}

	sinlge_t dot(const sinlge_vec<size>& other) const
	{
		return _dot<size>(other);
	}

	sinlge_vec<size> sum(const sinlge_vec<size>& other) const
	{
		return _sum<size>(other);
	}

	sinlge_vec<size> sub(const sinlge_vec<size>& other) const
	{
		return add(other.mul(-1.f)); 
	}

	sinlge_vec<size> crs(const sinlge_vec<size>& other) const { return _crs<size>(other); }

	sinlge_t mag2(void) const
	{
		return dot(*this); 
	}

	sinlge_t mag(void) const 
	{ 
		return sinlge_sqrt(mag2()); 
	}

	sinlge_vec<size> nor(void) const 
	{ 
		return mul(1.f / mag()); 
	}

	std::string to_string(void) const
	{
		std::ostringstream stream;
		
		for (size_t i = 0; i < size; i++)
		{
			stream << (i ? "," : "<") << _data[i];
		}

		stream << ">";

		return stream.str();
	}
};

template<size_t size>
struct sinlge_mat
{
	// this is the "only" data for this sucker
	sinlge_vec<size> _columns[size];

	template <size_t c, size_t r> sinlge_t& cell(void) const { return _columns[c - 1]._data[r - 1]; }

	sinlge_mat<size - 1> drop(const size_t col, const size_t row) const;
private: // private implmentation

	// ====
	// templates to calculate the identity matrix
	// ----
	template <size_t c>
	struct _identity_row
	{
		template <size_t r>
		static void _row(sinlge_mat<size>& result)
		{
			assert(c != 0 || r != 0);

			result._columns[c - 1]._data[r - 1] = c == r ? 1.f : 0.f;

			_row<r - 1>(result);
		}
		template <>
		static void _row<0>(sinlge_mat<size>& result)
		{
		}
	};
	template <>
	struct _identity_row<0>
	{
		template <size_t r>
		static void _row(sinlge_mat<size>& result)
		{
		}
	};

	template <size_t c>
	static sinlge_mat<size> _identity(void)
	{
		sinlge_mat<size> result = _identity<c - 1>();

		_identity_row<c>::_row<size>(result);

		return result;
	}
	template <>
	static sinlge_mat<size> _identity<0>(void)
	{
		return sinlge_mat<size>();
	}

	// ====
	// templates to extract a row
	// ----
	template<const size_t c>
	sinlge_vec<size> _row(const size_t r) const
	{
		assert(r != 0);

		sinlge_vec<size> result = _row<c - 1>(r);

		result._data[c - 1] = _columns[c - 1]._data[r - 1];

		return result;
	}

	template<>
	sinlge_vec<size> _row<0>(const size_t r) const
	{
		return sinlge_vec<size>();
	}


	// ====
	// templates to calculate the transformed vector
	// ----
	template<size_t component>
	sinlge_vec<size> _transform(const sinlge_vec<size>& vec) const
	{
		assert(component != 0);

		sinlge_vec<size> result = _transform<component - 1>(vec);


		sinlge_vec<size> row = _row<size>(component);

		result._data[component - 1] = row.dot(vec);

		return result;
	}
	template<>
	sinlge_vec<size> _transform<0>(const sinlge_vec<size>& vec) const
	{
		return sinlge_vec<size>();
	}


	// ====
	// templates to calculate the scaled matrix
	// ----
	template<size_t component>
	sinlge_mat<size> _scale(const sinlge_vec<size>& vec) const
	{
		sinlge_mat<size> result = _scale<component - 1>(vec);

		result._columns[component - 1]._data[component - 1] *= vec._data[component - 1];

		return result;
	}
	template<>
	sinlge_mat<size> _scale<0>(const sinlge_vec<size>& vec) const
	{
		return sinlge_mat<size>(*this);
	}


	// ====
	// templates to calculate the combined matrix product
	// ----
	template <size_t component>
	sinlge_mat<size> _product(const sinlge_mat<size>& other) const
	{
		sinlge_mat<size> result = _product<component - 1>(other);

		result._columns[component - 1] = other.transform(_columns[component - 1]);

		return result;
	}
	template <>
	sinlge_mat<size> _product<0>(const sinlge_mat<size>& other) const
	{
		return sinlge_mat<size>(*this);
	}


	// ====
	// templates to build a matrix
	// ----
	template <size_t c>
	static sinlge_mat<size> _var(const sinlge_t* vals)
	{
		sinlge_mat<size> result = _var<c - 1>(vals);

		result._columns[c - 1] = sinlge_vec<size>::arr(vals + (size * (c - 1)));

		return result;
	}
	template <>
	static sinlge_mat<size> _var<0>(const sinlge_t* vals)
	{
		return sinlge_mat<size>();
	}

	// ====
	// templates to check for matrix equality
	// ----
	template <size_t c>
	bool _equ(const sinlge_mat<size>& other) const
	{
		return _columns[c - 1].equ(other._columns[c - 1]) && _equ<c - 1>(other);
	}
	template <>
	bool _equ<0>(const sinlge_mat<size>& other) const
	{
		return true;
	}

	// ====
	// templates to check for matrix equality
	// ----
	template <size_t c>
	sinlge_mat<size> _transpose(void) const
	{
		sinlge_mat<size> result = _transpose<c - 1>();

		result._columns[c - 1] = _row<size>(c);

		return result;
	}
	template <> sinlge_mat<size> _transpose<0>(void) const { return sinlge_mat<size>(); }



	// ====
	// templates to drop row/column
	// ----
	template <size_t c, size_t r>
	sinlge_mat<size - 1> _drop(const size_t col, const size_t row)
	{
		sinlge_mat<size - 1> result = _drop<c - 1, r - 1>(col, row);

		if (c != col && r != row)
		{
			result._columns[(c < col) ? (c - 1) : (c - 2)] = _columns[c - 1].drop(row);
		}

		return result;
	}
	template <> sinlge_mat<size - 1> _drop<0, 0>(const size_t col, const size_t row) { return sinlge_mat<size - 1>(); }


	// ====
	// templates to calculate determinant
	// ----
	
	template <size_t c>
	sinlge_t _determinant(void) const;

	template <>
	sinlge_t _determinant<3>(void) const
	{
		return
			(get<1, 1>() * drop(1, 1))
			- (get<1, 2>() * drop(1, 2))
			+ (get<1, 3>() * drop(1, 3));
	}

	template <>
	sinlge_t _determinant<2>(void) const
	{
		const auto a = _columns[0]._data[0];
		const auto b = _columns[1]._data[0];
		const auto c = _columns[0]._data[1];
		const auto d = _columns[1]._data[1];

		return (a * d) - (b * c);
	}

	template <>	sinlge_t _determinant<1>(void) const { return  _columns[0]._data[0]; }

public: // public API
	sinlge_mat<size - 1> drop(const size_t col, const size_t row) const
	{
		return _drop<size>(col, row);
	}

	sinlge_t determinant(void) const
	{
		return _determinant<size>();
	}

	bool equ(const sinlge_mat<size>& other) const
	{
		return _equ<size>(other);
	}

	static sinlge_mat<size> identity(void)
	{
		return _identity<size>();
	}

	/*
	 * Constructor for how it's laid out in memory
	*/
	static sinlge_mat<size> var_column(const std::initializer_list<sinlge_t>& vals)
	{
		assert((size * size) == vals.size());

		return _var<size>(vals.begin());
	}


	/*
	 * Constructor for how it's laid out on paper
	*/
	static sinlge_mat<size> var_row(const std::initializer_list<sinlge_t>& vals)
	{
		assert((size * size) == vals.size());

		return var_column(vals).transpose();
	}

	sinlge_vec<size> transform(const sinlge_vec<size>& vec) const
	{
		return _transform<size>(vec);
	}

	sinlge_mat<size> translate(const sinlge_vec<size - 1>& vec) const
	{
		sinlge_mat<size> result = *this;

		result._columns[size - 1] = _columns[size - 1].sum(vec.ext(0.f));

		return result;
	}

	sinlge_mat<size> scale(const sinlge_vec<size>& vec) const { return _scale<size>(vec); }
	sinlge_mat<size> scale(const sinlge_vec<size - 1>& vec) { return scale(vec.ext(1.f)); }

	sinlge_mat<size> transpose(void) const { return _transpose<size>(); }
	sinlge_mat<size> product(const sinlge_mat<size>& other) const { return _product<size>(other); }
	
	// sinlge_mat<size> summation(const sinlge_mat<size>&) const;
	// sinlge_mat<size> multiply(const sinlge_t&) const;


	std::string to_string(void) const
	{
		std::ostringstream stream;

		stream << "[";
		for (size_t i = 0; i < size; ++i)
		{
			stream << _columns[i].to_string();
		}
		stream << "]";

		return stream.str();
	}
};