diff --git a/src/modm/math/geometry/geometric_traits.hpp b/src/modm/math/geometry/geometric_traits.hpp
index fd5528c94c..b61af32602 100644
--- a/src/modm/math/geometry/geometric_traits.hpp
+++ b/src/modm/math/geometry/geometric_traits.hpp
@@ -1,6 +1,7 @@
 /*
  * Copyright (c) 2009-2011, Fabian Greif
  * Copyright (c) 2012, Niklas Hauser
+ * Copyright (c) 2022, Thomas Sommer
  *
  * This file is part of the modm project.
  *
@@ -10,12 +11,13 @@
  */
 // ----------------------------------------------------------------------------
 
-#ifndef MODM_GEOMETRIC_TRAITS_HPP
-#define MODM_GEOMETRIC_TRAITS_HPP
+#pragma once
 
+#include <concepts>
 #include <cmath>
-#include <stdint.h>
+#include <limits>
 #include <modm/architecture/utils.hpp>
+#include <modm/math/utils/arithmetic_traits.hpp>
 
 namespace modm
 {
@@ -24,120 +26,42 @@ namespace modm
 	 *
 	 * \ingroup	modm_math_geometry
 	 * \author	Fabian Greif
+	 * \author	Thomas Sommer
 	 */
 	template <typename T>
-	struct GeometricTraits
-	{
-		static const bool isValidType = false;
+	struct GeometricTraits;
 
-		/**
-		 * \brief	Round if converting from a floating point base to
-		 * 			a integer base.
-		 *
-		 * For T = \c float and \c double this method is specialized to return
-		 * the result directly without any rounding.
-		 */
-		static inline T
-		round(float value)
-		{
-			return ::round(value);
-		}
-	};
-
-	template <>
-	struct GeometricTraits<int8_t>
+	template <std::integral T>
+	struct GeometricTraits<T>
 	{
-		static const bool isValidType = true;
-
-		typedef float FloatType;
-		typedef int16_t WideType;
-
-		static inline int8_t
-		round(float value)
-		{
-			return ::round(value);
-		}
-	};
-
-	// TODO is this useful?
-	template <>
-	struct GeometricTraits<uint8_t>
-	{
-		static const bool isValidType = true;
-
-		typedef float FloatType;
-		typedef int16_t WideType;
+		[[deprecated("Use an appropriate C++ concept instead!")]]
+		static const bool isValidType = false;
 
-		static inline uint8_t
-		round(float value)
-		{
-			return ::round(value);
-		}
+		using FloatType = float;
+		using WideType = modm::WideType<T>;
 	};
 
-	template <>
-	struct GeometricTraits<int16_t>
+	template <std::floating_point T>
+	struct GeometricTraits<T>
 	{
+		[[deprecated("Use an appropriate C++ concept instead!")]]
 		static const bool isValidType = true;
 
-		typedef float FloatType;
-		typedef int32_t WideType;
-
-		static inline int16_t
-		round(float value)
-		{
-			return ::round(value);
-		}
+		using FloatType = T;
+		using WideType = T;
 	};
 
+	#ifdef __AVR__
 	template <>
 	struct GeometricTraits<int32_t>
 	{
+		[[deprecated("Use an appropriate C++ concept instead!")]]
 		static const bool isValidType = true;
 
-		typedef float FloatType;
-
-		// Usually the range of a int32_t is big enough so that no
+		using FloatType = float;
 		// conversion to int64_t is required. This exception is made because
 		// 64-bit operations are very, very slow on an AVR.
-		typedef int32_t WideType;
-
-		static inline int32_t
-		round(float value)
-		{
-			return ::round(value);
-		}
-	};
-
-	template <>
-	struct GeometricTraits<float>
-	{
-		static const bool isValidType = true;
-
-		typedef float FloatType;
-		typedef float WideType;
-
-		static inline float
-		round(float value)
-		{
-			return value;
-		}
+		using WideType = int32_t;
 	};
-
-	template <>
-	struct GeometricTraits<double>
-	{
-		static const bool isValidType = true;
-
-		typedef double FloatType;
-		typedef double WideType;
-
-		static inline double
-		round(double value)
-		{
-			return value;
-		}
-	};
-}
-
-#endif // MODM_GEOMETRIC_TRAITS_HPP
+	#endif
+}
\ No newline at end of file
diff --git a/src/modm/math/geometry/location_2d.hpp b/src/modm/math/geometry/location_2d.hpp
index c0a350e019..8eff73ceb7 100644
--- a/src/modm/math/geometry/location_2d.hpp
+++ b/src/modm/math/geometry/location_2d.hpp
@@ -3,6 +3,8 @@
  * Copyright (c) 2009-2011, Fabian Greif
  * Copyright (c) 2012, Niklas Hauser
  * Copyright (c) 2012, Sascha Schade
+ * Copyright (c) 2013, Kevin Läufer
+ * Copyright (c) 2022, Thomas Sommer
  *
  * This file is part of the modm project.
  *
@@ -12,10 +14,7 @@
  */
 // ----------------------------------------------------------------------------
 
-#ifndef	MODM_LOCATION_2D_HPP
-#define	MODM_LOCATION_2D_HPP
-
-#include <cmath>
+#pragma once
 
 #include <modm/io/iostream.hpp>
 
@@ -37,39 +36,60 @@ namespace modm
 	class Location2D
 	{
 	public:
-		Location2D();
-
-		Location2D(const Vector<T, 2>& position, const float& orientation);
-
-		Location2D(const T& x, const T& y, const float& orientation);
-
-		inline const Vector<T, 2>&
-		getPosition() const;
-
-		inline const T&
-		getX() const;
-
-		inline const T&
-		getY() const;
-
-		void
-		setPosition(const Vector<T, 2>& point);
-
-		void
-		setPosition(const T& x, const T& y);
+		Vector<T, 2> position;
+		float orientation = 0;
+
+		constexpr Location2D() = default;
+		constexpr Location2D(const Vector<T, 2>& position, const float& orientation)
+			: position(position), orientation(orientation) {}
+
+		[[deprecated("Use 'setPosition({x, y}, orientation)' instead!")]]
+		constexpr Location2D(const T& x, const T& y, const float& orientation)
+			: position(x, y), orientation(orientation) {}
+
+		template<typename U>
+		constexpr Location2D(const Location2D<U> &l) : position(l.position), orientation(l.orientation) {}
+
+		// getters and setters
+		void setPosition(const Vector<T, 2>& position) { this->position = position; }
+
+		[[deprecated("Use 'setPosition({x, y}' instead!")]]
+		void setPosition(T x, T y) { this->position.x = x; this->position.y = y; }
+		void setOrientation(const float orientation) { this->orientation = orientation; }
+
+		Vector<T, 2> getPosition() const { return position; }
+		inline float getOrientation() const { return orientation; }
+		T getX() const { return position.x; }
+		T getY() const { return position.y; }
+
+		bool operator== (const Location2D &other) const {
+			return (
+				position == other.position and
+				std::abs(orientation - other.orientation) < __FLT_EPSILON__
+			);
+		}
+		bool operator!= (const Location2D &other) const {
+			return (
+				position != other.position or
+				std::abs(orientation - other.orientation) > __FLT_EPSILON__
+			);
+		}
 
-		inline float
-		getOrientation() const;
+		/// Add a position increment
+		void move(const Location2D& diff) {
+			Vector<T, 2> movement = diff.position;
+			movement.rotate(orientation);
 
-		void
-		setOrientation(const float& phi);
+			position.translate(movement);
+			orientation = Angle::normalize(orientation + diff.orientation);
+		}
 
-		/// Add a position increment
-		void
-		move(const Location2D& diff);
+		void move(const Vector<T, 2>& diff) {
+			Vector<T, 2> movement(diff);
+			movement.rotate(orientation);
 
-		void
-		move(const Vector<T, 2>& diff);
+			position.translate(movement);
+		}
 
 		/**
 		 * \brief	Add a increment only in x-direction
@@ -85,62 +105,38 @@ namespace modm
 		 * movement over time.
 		 * Because the y-component will always be zero, we created this
 		 * method, which avoids unnecessary computations for the y-component
-		 * and is therefore faster the the universal move-method.
+		 * and is therefore faster than the universal move-method.
 		 *
 		 * \param	x		movement in x-direction
 		 * \param	phi		rotation
 		 */
 		void
-		move(T x, float phi);
+		move(T x, float phi) {
+			Vector<T, 2> vector(Vector<float, 2>(x * std::cos(orientation), x * std::sin(orientation)));
+			position.translate(vector);
 
-		/// TODO
-		Vector<T, 2>
-		translated(const Vector<T, 2>& vector) const;
-
-		/// Convert between Location-objects with different base-types
-		template <typename U>
-		Location2D<U>
-		convert() const;
+			orientation = Angle::normalize(orientation + phi);
+		}
 
-		bool
-		operator == (const Location2D &other) const;
+		// TODO
+		Vector<T, 2> translated(const Vector<T, 2>& vector) const {
+			Vector<T, 2> result(vector);
+			result.rotate(orientation);
+			result.translate(position);
 
-		bool
-		operator != (const Location2D &other) const;
+			return result;
+		}
 
 	private:
 		template <typename U>
 		friend IOStream&
 		operator <<( IOStream&, const Location2D<U>&);
-
-		Vector<T, 2> position;
-		float orientation;
 	};
 
-	// ------------------------------------------------------------------------
-	// Global functions
-	// ------------------------------------------------------------------------
-	/**
-	 * \brief	Stream operator to \b modm::Location<T>
-	 *
-	 * \ingroup	modm_math_geometry
-	 */
 	template<typename T>
 	IOStream&
-	operator << (IOStream& os, const Location2D<T>& l);
-
-	// ------------------------------------------------------------------------
-	// Declaration of specialized methods
-	// ------------------------------------------------------------------------
-	/*template<>
-	bool
-	Location2D<float>::operator == (const Location2D &other) const;
-
-	template<>
-	bool
-	Location2D<double>::operator == (const Location2D &other) const;*/
-}
-
-#include "location_2d_impl.hpp"
-
-#endif	// MODM_LOCATION_2D_HPP
+	operator<< (IOStream& os, const Location2D<T>& location) {
+		os << location.position << ", phi=" << location.orientation;
+		return os;
+	}
+}
\ No newline at end of file
diff --git a/src/modm/math/geometry/location_2d_impl.hpp b/src/modm/math/geometry/location_2d_impl.hpp
deleted file mode 100644
index 913449fcd7..0000000000
--- a/src/modm/math/geometry/location_2d_impl.hpp
+++ /dev/null
@@ -1,170 +0,0 @@
-/*
- * Copyright (c) 2009-2010, Martin Rosekeit
- * Copyright (c) 2009-2011, Fabian Greif
- * Copyright (c) 2013, Kevin Läufer
- *
- * This file is part of the modm project.
- *
- * This Source Code Form is subject to the terms of the Mozilla Public
- * License, v. 2.0. If a copy of the MPL was not distributed with this
- * file, You can obtain one at http://mozilla.org/MPL/2.0/.
- */
-// ----------------------------------------------------------------------------
-
-#ifndef	MODM_LOCATION_2D_HPP
-	#error	"Don't include this file directly use 'location.hpp' instead!"
-#endif
-
-#include <cmath>
-#include <cstdlib>
-
-// -----------------------------------------------------------------------------
-template <typename T>
-modm::Location2D<T>::Location2D() :
-	position(), orientation()
-{
-}
-
-template <typename T>
-modm::Location2D<T>::Location2D(const Vector<T, 2>& position,
-		const float& orientation) :
-	position(position),
-	orientation(orientation)
-{
-}
-
-template <typename T>
-modm::Location2D<T>::Location2D(const T& x, const T& y, const float& orientation) :
-	position(x, y),
-	orientation(orientation)
-{
-}
-
-// ----------------------------------------------------------------------------
-template <typename T>
-const modm::Vector<T, 2>&
-modm::Location2D<T>::getPosition() const
-{
-	return this->position;
-}
-
-template <typename T>
-inline const T&
-modm::Location2D<T>::getX() const
-{
-	return this->position.x;
-}
-
-template <typename T>
-inline const T&
-modm::Location2D<T>::getY() const
-{
-	return this->position.y;
-}
-
-template <typename T>
-void
-modm::Location2D<T>::setPosition(const Vector<T, 2>& point)
-{
-	this->position = point;
-}
-
-template <typename T>
-void
-modm::Location2D<T>::setPosition(const T& x, const T& y)
-{
-	this->position.set(x, y);
-}
-
-template <typename T>
-float
-modm::Location2D<T>::getOrientation() const
-{
-	return this->orientation;
-}
-
-template <typename T>
-void
-modm::Location2D<T>::setOrientation(const float& orientation)
-{
-	this->orientation = orientation;
-}
-
-// -----------------------------------------------------------------------------
-template <typename T>
-void
-modm::Location2D<T>::move(const Location2D<T>& diff)
-{
-	Vector<T, 2> movement = diff.position;
-	movement.rotate(this->orientation);
-
-	this->position.translate(movement);
-	this->orientation = Angle::normalize(this->orientation + diff.orientation);
-}
-
-template <typename T>
-void
-modm::Location2D<T>::move(const Vector<T, 2>& diff)
-{
-	Vector<T, 2> movement(diff);
-	movement.rotate(this->orientation);
-
-	this->position.translate(movement);
-}
-
-template <typename T>
-void
-modm::Location2D<T>::move(T x, float phi)
-{
-	Vector<T, 2> vector(GeometricTraits<T>::round(x * std::cos(this->orientation)),
-					   GeometricTraits<T>::round(x * std::sin(this->orientation)));
-	position.translate(vector);
-
-	this->orientation = Angle::normalize(this->orientation + phi);
-}
-
-// ----------------------------------------------------------------------------
-template <typename T>
-modm::Vector<T, 2>
-modm::Location2D<T>::translated(const Vector<T, 2>& vector) const
-{
-	Vector<T, 2> result(vector);
-	result.rotate(this->orientation);
-	result.translate(this->position);
-
-	return result;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T> template<typename U>
-modm::Location2D<U>
-modm::Location2D<T>::convert() const
-{
-	return Location2D<U>(this->position.template convert<U>(), this->orientation);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Location2D<T>::operator == (const Location2D &other) const
-{
-	return ((this->position == other.position) &&
-			(std::abs(this->orientation - other.orientation) < __FLT_EPSILON__));
-}
-
-template<typename T>
-bool
-modm::Location2D<T>::operator != (const Location2D &other) const
-{
-	return ((this->position != other.position) ||
-			(std::abs(this->orientation - other.orientation) > __FLT_EPSILON__));
-}
-
-// ----------------------------------------------------------------------------
-template<class T>
-modm::IOStream&
-modm::operator << (modm::IOStream& os, const modm::Location2D<T>& location)
-{
-	os << location.position << ", phi=" << location.orientation;
-	return os;
-}
diff --git a/src/modm/math/geometry/quaternion.hpp b/src/modm/math/geometry/quaternion.hpp
index d3cbcdb761..879ca8847a 100644
--- a/src/modm/math/geometry/quaternion.hpp
+++ b/src/modm/math/geometry/quaternion.hpp
@@ -11,8 +11,7 @@
  */
 // ----------------------------------------------------------------------------
 
-#ifndef MODM_QUATERNION_HPP
-#define MODM_QUATERNION_HPP
+#pragma once
 
 #include <cmath>
 #include <stdint.h>
@@ -20,10 +19,10 @@
 namespace modm
 {
 	// forward declaration
-	template<class T, uint8_t N>
+	template<class T, std::size_t N>
 	class Vector;
 
-	template<class T, uint8_t ROWS, uint8_t COLUMNS>
+	template<class T, std::size_t ROWS, std::size_t COLUMNS>
 	class Matrix;
 
 	/**
@@ -111,5 +110,3 @@ namespace modm
 }
 
 #include "quaternion_impl.hpp"
-
-#endif	// MODM_QUATERNION_HPP
diff --git a/src/modm/math/geometry/quaternion_impl.hpp b/src/modm/math/geometry/quaternion_impl.hpp
index 0fa1b4e24e..2cac396087 100644
--- a/src/modm/math/geometry/quaternion_impl.hpp
+++ b/src/modm/math/geometry/quaternion_impl.hpp
@@ -9,15 +9,10 @@
  * License, v. 2.0. If a copy of the MPL was not distributed with this
  * file, You can obtain one at http://mozilla.org/MPL/2.0/.
  */
-// ----------------------------------------------------------------------------
-
-#ifndef MODM_QUATERNION_HPP
-#	error	"Don't include this file directly, use 'quaternion.hpp' instead!"
-#endif
 
+#pragma once
 #include "quaternion.hpp"
 
-// ----------------------------------------------------------------------------
 template<class T>
 modm::Quaternion<T>::Quaternion()
 :
diff --git a/src/modm/math/geometry/vector.hpp b/src/modm/math/geometry/vector.hpp
index 20f918e37c..208dc778c6 100644
--- a/src/modm/math/geometry/vector.hpp
+++ b/src/modm/math/geometry/vector.hpp
@@ -3,6 +3,7 @@
  * Copyright (c) 2012, Georgi Grinshpun
  * Copyright (c) 2012, Martin Rosekeit
  * Copyright (c) 2012, Niklas Hauser
+ * Copyright (c) 2022, Thomas Sommer
  *
  * This file is part of the modm project.
  *
@@ -12,140 +13,227 @@
  */
 // ----------------------------------------------------------------------------
 
-#ifndef MODM_VECTOR_HPP
-#define MODM_VECTOR_HPP
+#pragma once
 
 #include <cmath>
-#include <stdint.h>
-
+#include <numeric>
+#include <algorithm>
 #include <modm/math/matrix.hpp>
-#include <modm/math/utils/arithmetic_traits.hpp>
+
+#include "geometric_traits.hpp"
 
 namespace modm
 {
-	// forward declaration
-	template<typename T, uint8_t W, uint8_t H> class Matrix;
-
-	/**
-	 * \brief	Class for handling common point operations
-	 *
-	 * Basic data type of all geometric operations. Used to represent vectors
-	 * as well as particular points in the coordinate system.
-	 *
-	 * \section	point_vector	Point vs. vector
-	 *
-	 * In geometry, it is often convenient to use vector arithmetic to
-	 * represent points.
-	 *
-	 * A vector, by its definition, has no fixed starting point, but if we
-	 * imagine the starting point of a vector to be the origin, then the
-	 * endpoint of the vector represents a particular point.
-	 *
-	 * In this manner, every vector can be said to identify a unique point,
-	 * which is the endpoint of the vector when its starting point is the
-	 * origin.
-	 *
-	 * Therefore there isn't a Point-class, but only a Vector class.
-	 *
-	 * Adapted from the implementation of Gaspard Petit (gaspardpetit@gmail.com).
-	 *
-	 * \see <a href"http://www-etud.iro.umontreal.ca/~petitg/cpp/point.html">Homepage</a>
-	 *
-	 * \ingroup	modm_math_geometry
-	 * \author	Niklas Hauser
-	 */
-	template<typename T, uint8_t N>
-	class Vector
-	{
-	public:
-		Vector();
-		Vector(const T *ptData);
-
-		Vector(const Matrix<T, N, 1> &rhs);
-		Vector& operator = (const Matrix<T, N, 1> &rhs);
-
-		bool operator == (const Vector &rhs) const;
-		bool operator != (const Vector &rhs) const;
-		bool operator < (const Vector &rhs) const;
-		bool operator <= (const Vector &rhs) const;
-		bool operator > (const Vector &rhs) const;
-		bool operator >= (const Vector &rhs) const;
-
-		const T& operator [] (uint8_t index) const;
-		T& operator [] (uint8_t index);
-
-		T* ptr();
-		const T* ptr() const;
-
-		Vector operator + (const Vector &rhs) const;
-		Vector operator - (const Vector &rhs) const;
-		T operator * (const Vector &rhs) const;
-		Vector operator * (const T &rhs) const;
-		Vector operator / (const T &rhs) const;
-		Vector& operator += (const Vector &rhs);
-		Vector& operator -= (const Vector &rhs);
-		Vector& operator *= (const T &rhs);
-		Vector& operator /= (const T &rhs);
-		Vector& operator - ();		// FIXME
-
-		T getLength() const;
-		T getLengthSquared() const;
-
-		Matrix<T, N, 1>&
-		asMatrix();
-
-		const Matrix<T, N, 1>&
-		asMatrix() const;
-
-		Matrix<T, 1, N>&
-		asTransposedMatrix();
-
-		const Matrix<T, 1, N>&
-		asTransposedMatrix() const;
-
-	public:
-		static inline uint8_t
-		getSize();
-
-		T coords[N];
-	};
-
-	template< typename T, uint8_t N >
-	struct detail::MakeSigned< Vector<T, N> >
+// forward declaration
+template<typename T, std::size_t, std::size_t>
+class Matrix;
+
+/**
+ * \brief	Class for handling common point operations
+ *
+ * Basic data type of all geometric operations. Used to represent vectors
+ * as well as particular points in the coordinate system.
+ *
+ * \section	point_vector	Point vs. vector
+ *
+ * In geometry, it is often convenient to use vector arithmetic to
+ * represent points.
+ *
+ * A vector, by its definition, has no fixed starting point, but if we
+ * imagine the starting point of a vector to be the origin, then the
+ * endpoint of the vector represents a particular point.
+ *
+ * In this manner, every vector can be said to identify a unique point,
+ * which is the endpoint of the vector when its starting point is the
+ * origin.
+ *
+ * Therefore there isn't a Point-class, but only a Vector class.
+ *
+ * Adapted from the implementation of Gaspard Petit (gaspardpetit@gmail.com).
+ *
+ * \see <a href"http://www-etud.iro.umontreal.ca/~petitg/cpp/point.html">Homepage</a>
+ *
+ * \ingroup	modm_math_geometry
+ * \author	Niklas Hauser
+ * \author	Thomas Sommer
+ */
+template<typename T, std::size_t N>
+class Vector
+{
+public:
+	static constexpr std::size_t size = N;
+
+	T coords[N] = {0};
+
+	// fundamental constructors
+	constexpr Vector() = default;
+
+	constexpr explicit Vector(T v)
+	{ std::fill(coords, coords + N, v); }
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	constexpr explicit Vector(U v)
+	{ std::fill(coords, coords + N, std::round(v)); }
+
+	constexpr Vector(const T (&arr)[N])
+	{ std::copy(arr, arr + N, coords); }
+
+	// TODO need std::round
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	constexpr Vector(const U (&arr)[N])
+	{ std::copy(arr, arr + N, coords); }
+
+	// matrix constructor
+	constexpr Vector(const Matrix<T, N, 1> &rhs)
+	{ std::copy(rhs, rhs + N, coords); }
+
+	// as matrix convertors
+	Matrix<T, N, 1>&
+	asMatrix()
+	{ return *reinterpret_cast<modm::Matrix<T, N, 1>*>(this); }
+
+	const Matrix<T, N, 1>&
+	asMatrix() const
+	{ return *reinterpret_cast<const modm::Matrix<T, N, 1>*>(this); }
+
+	Matrix<T, 1, N>&
+	asTransposedMatrix()
+	{ return *reinterpret_cast<const modm::Matrix<T, 1, N>*>(this); }
+
+	const Matrix<T, 1, N>&
+	asTransposedMatrix() const
+	{ return *reinterpret_cast<modm::Matrix<T, 1, N>*>(this); }
+
+	// matrix assignment
+	Vector& operator= (const Matrix<T, N, 1> &rhs) {
+		std::copy(coords, coords + N, &rhs);
+		return *this;
+	}
+
+	// accessors
+	T& operator [] (std::size_t index)
+	{ return coords[index]; }
+
+	const T& operator [] (std::size_t index) const
+	{ return coords[index]; }
+
+	T* ptr() { return coords; }
+
+	const T* ptr() const { return reinterpret_cast<const T*>(coords); }
+
+	// operators
+	auto operator<=>(const Vector &) const = default;
+
+	Vector operator+ (const Vector &rhs) const {
+		Vector ret;
+		std::transform(coords, coords + N, rhs.coords, ret.coords, std::plus<T>());
+		return ret;
+	}
+
+	Vector operator- (const Vector &rhs) const {
+		Vector ret;
+		std::transform(coords, coords + N, rhs.coords, ret.coords, std::minus<T>());
+		return ret;
+	}
+
+	T operator* (const Vector &rhs) const {
+		T tmp[N];
+		std::transform(coords, coords + N, rhs.coords, tmp, std::multiplies<T>());
+		return std::accumulate(tmp, tmp + N, 0);
+	}
+
+	Vector operator* (const T &rhs) const {
+		Vector ret;
+		std::transform(coords, coords + N, ret.coords, [=] (T c) {return c * rhs;});
+		return ret;
+	}
+
+	Vector operator/ (const T &rhs) const {
+		Vector ret;
+		std::transform(coords, coords + N, ret.coords, [=] (T c) {return c / rhs;});
+		return ret;
+	}
+
+	Vector& operator+= (const Vector &rhs) {
+		std::transform(coords, coords + N, rhs.coords, coords, std::plus<T>());
+		return *this;
+	}
+
+	Vector& operator-= (const Vector &rhs) {
+		std::transform(coords, coords + N, rhs.coords, coords, std::minus<T>());
+		return *this;
+	}
+
+	Vector& operator*= (const T &rhs) {
+		std::transform(coords, coords + N, coords, [=] (T c) {return c * rhs;});
+		return *this;
+	}
+
+	Vector& operator/= (const T &rhs) {
+		std::transform(coords, coords + N, coords, [=] (T c) {return c / rhs;});
+		return *this;
+	}
+
+	// template <typename>
+	// requires std::is_signed<T>::value
+	constexpr Vector operator- () {
+		Vector ret;
+		std::transform(coords, coords + N, ret.coords, std::negate<T>());
+		return ret;
+	}
+
+	// additional methods
+	T getLength() const
+	{ return std::sqrt(getLengthSquared()); }
+
+	T getLengthSquared() const {
+		T tmp[N];
+		// Better do std::pow(c, 2);
+		std::transform(coords, coords + N, coords, tmp, std::multiplies<T>());
+		return std::accumulate(tmp, tmp + N, 0);
+	}
+
+	// IMPLEMENT operator<<
+};
+
+namespace detail {
+	template< typename T, std::size_t N >
+	struct MakeSigned< Vector<T, N> >
 	{ using type = Vector< SignedType<T>, N >; };
 
-	template< typename T, uint8_t N >
-	struct detail::MakeUnsigned< Vector<T, N> >
+	template< typename T, std::size_t N >
+	struct MakeUnsigned< Vector<T, N> >
 	{ using type = Vector< UnsignedType<T>, N >; };
 
-	template< typename T, uint8_t N >
-	struct detail::WideType< Vector<T, N> >
+	template< typename T, std::size_t N >
+	struct WideType< Vector<T, N> >
 	{ using type = Vector< WideType<T>, N >; };
-}
 
-#define IMPLEMENT_VECTOR_ACCESSOR2(a,b)			\
-		Vector<T, 2> a##b() const				\
-		{										\
-			return Vector<T, 2>(a, b);			\
-		}
+} // namespace detail
 
-#define IMPLEMENT_VECTOR_ACCESSOR3(a, b, c)		\
-		Vector<T, 3> a##b##c() const			\
-		{										\
-			return Vector<T, 3>(a, b, c);		\
-		}
+} // namespace modm	
 
-#define IMPLEMENT_VECTOR_ACCESSOR4(a, b, c, d)	\
-		Vector<T, 4> a##b##c##d() const			\
-		{										\
-			return Vector<T, 4>(a, b, c, d);	\
-		}
+#define IMPLEMENT_VECTOR_ACCESSOR2(a,b)		\
+	Vector<T, 2> a##b() const				\
+	{										\
+		return Vector<T, 2>(a, b);			\
+	}
 
-#include "vector_impl.hpp"
+#define IMPLEMENT_VECTOR_ACCESSOR3(a, b, c)	\
+	Vector<T, 3> a##b##c() const			\
+	{										\
+		return Vector<T, 3>(a, b, c);		\
+	}
+
+#define IMPLEMENT_VECTOR_ACCESSOR4(a, b, c, d)\
+	Vector<T, 4> a##b##c##d() const			\
+	{										\
+		return Vector<T, 4>(a, b, c, d);	\
+	}
 
 #include "vector1.hpp"
 #include "vector2.hpp"
 #include "vector3.hpp"
-#include "vector4.hpp"
-
-#endif // MODM_VECTOR_HPP
+#include "vector4.hpp"
\ No newline at end of file
diff --git a/src/modm/math/geometry/vector1.hpp b/src/modm/math/geometry/vector1.hpp
index 923879e033..c5107510f8 100644
--- a/src/modm/math/geometry/vector1.hpp
+++ b/src/modm/math/geometry/vector1.hpp
@@ -2,6 +2,7 @@
  * Copyright (c) 2009-2010, Martin Rosekeit
  * Copyright (c) 2009-2012, Fabian Greif
  * Copyright (c) 2011-2012, Niklas Hauser
+ * Copyright (c) 2022, Thomas Sommer
  *
  * This file is part of the modm project.
  *
@@ -11,103 +12,179 @@
  */
 // ----------------------------------------------------------------------------
 
-#ifndef MODM_VECTOR1_HPP
-#define MODM_VECTOR1_HPP
+#pragma once
 
 #include <cmath>
 #include <cstdlib>
-#include <stdint.h>
+#include <concepts>
+#include <numeric>
 
 #include "vector.hpp"
 
 namespace modm
 {
-	/**
-	 * \brief	Class for handling common vector operations (1D)
-	 *
-	 * Adapted from the implementation of Gaspard Petit (gaspardpetit@gmail.com).
-	 *
-	 * \see <a href"http://www-etud.iro.umontreal.ca/~petitg/cpp/point.html">Homepage</a>
-	 *
-	 * \ingroup	modm_math_geometry
-	 * \author	Niklas Hauser
-	 */
-	template<typename T>
-	class Vector<T, 1>
-	{
-	public:
-		Vector();
-		Vector(T inX);
-		Vector(const Matrix<T, 1, 1> &rhs);
-
-		inline void
-		set(const T& x);
-
-		inline void
-		setX(const T& value);
-
-		inline const T&
-		getX() const;
-
-		Vector& operator = (const Matrix<T, 1, 1> &rhs);
-
-		bool operator == (const Vector &rhs) const;
-		bool operator != (const Vector &rhs) const;
-		bool operator < (const Vector &rhs) const;
-		bool operator <= (const Vector &rhs) const;
-		bool operator > (const Vector &rhs) const;
-		bool operator >= (const Vector &rhs) const;
-
-		const T& operator [] (uint8_t index) const;
-		T& operator [] (uint8_t index);
-		T* ptr();
-		const T* ptr() const;
-
-		Vector operator - () const;
-		Vector operator + (const Vector &rhs) const;
-		Vector operator - (const Vector &rhs) const;
-		T operator * (const Vector &rhs) const;
-		Vector operator * (const T &rhs) const;
-		Vector operator / (const T &rhs) const;
-
-		Vector& operator += (const Vector &rhs);
-		Vector& operator -= (const Vector &rhs);
-		Vector& operator *= (const T &rhs);
-		Vector& operator /= (const T &rhs);
-
-		T getLength() const;
-		T getLengthSquared() const;
-
-		Matrix<T, 1, 1>&
-		asMatrix();
-
-		const Matrix<T, 1, 1>&
-		asMatrix() const;
-
-		bool hasNan() const;
-		bool hasInf() const;
-
-	public:
-		T x;
-
-	public:
-	#ifndef __DOXYGEN__
-		IMPLEMENT_VECTOR_ACCESSOR2(x,x)
-		IMPLEMENT_VECTOR_ACCESSOR3(x,x,x)
-		IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,x)
-	#endif
-	};
-
-	template<typename U, typename T>
-	static inline Vector<T, 1> operator * (const U &lhs, const Vector<T, 1> &rhs)
-	{
-		return rhs * lhs;
+/**
+ * \brief	Class for handling common vector operations (1D)
+ *
+ * Adapted from the implementation of Gaspard Petit (gaspardpetit@gmail.com).
+ *
+ * \see <a href"http://www-etud.iro.umontreal.ca/~petitg/cpp/point.html">Homepage</a>
+ *
+ * \author	Niklas Hauser
+ * \author	Thomas Sommer
+ * \ingroup	modm_math_geometry
+ */
+template<typename T>
+class Vector<T, 1>
+{
+public:
+	using WideType = GeometricTraits<T>::WideType;
+	using FloatType = GeometricTraits<T>::FloatType;
+
+	T x{0};
+
+	// fundamental constructors
+	constexpr Vector() = default;
+
+	constexpr Vector(T x) : x(x) {}
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	constexpr Vector(U x) : x(std::round(x)) {}
+
+	template<typename U>
+	constexpr Vector(Vector<U, 1> v) : Vector(v.x) {}
+
+	// matrix constructors
+	template<typename U>
+	constexpr Vector(const Matrix<U, 1, 1> &rhs)
+		: Vector(*reinterpret_cast<const T*>(&rhs)) {}
+
+	// as matrix convertors
+	Matrix<T, 1, 1>&
+	asMatrix() { return *(modm::Matrix<T, 1, 1>*) this; }
+
+	const Matrix<T, 1, 1>&
+	asMatrix() const { return *(modm::Matrix<T, 1, 1>*) this; }
+
+	// matrix assignment
+	Vector& operator= (const Matrix<T, 1, 1> &rhs) {
+		x = *reinterpret_cast<const T*>(&rhs);
+		return *this;
 	}
 
-	typedef Vector<float, 1> 	Vector1f;
-	typedef Vector<int16_t, 1> 	Vector1i;
+	// getters
+	T getX() const { return x; }
+
+	// setters
+	void setX(T x) { this->x = x; }
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	void setX(U x) { this->x = std::round(x); }
+
+	void set(T x) { this->x = x; }
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	void set(U x) { this->x = std::round(x); }
+
+	// accessors
+	T& operator[] (std::size_t index)
+	{ return reinterpret_cast<T*>(this)[index]; }
+
+	const T& operator[] (std::size_t index) const
+	{ return reinterpret_cast<const T*>(this)[index]; }
+
+	T* ptr() { return reinterpret_cast<T*>(this); }
+
+	const T* ptr() const { return reinterpret_cast<const T*>(this);}
+
+	// operators
+	auto operator<=>(const Vector &) const = default;
+
+	constexpr Vector operator+ (const Vector &rhs) const
+	{ return Vector(x + rhs.x); }
+
+	constexpr Vector operator- (const Vector &rhs) const
+	{ return Vector(x - rhs.x); }
+
+	constexpr T operator* (const Vector &rhs) const
+	{ return x * rhs.x; }
+
+	template <typename U>
+	constexpr Vector operator* (U scale) const
+	{ return Vector<U, 1>(x * scale); }
+
+	template <typename U>
+	constexpr Vector operator/ (U scale) const
+	{ return Vector<U, 1>(x / scale); }
+
+	Vector& operator+= (const Vector &rhs)
+	{ x += rhs.x; return *this; }
+
+	Vector& operator-= (const Vector &rhs)
+	{ x -= rhs.x; return *this; }
+
+	template <typename U>
+	Vector& operator*= (U scale)
+	{ x *= scale; return *this; }
+
+	template <typename U>
+	Vector& operator/= (U scale)
+	{ x /= scale; return *this; }
+
+	// template <typename>
+	// requires std::is_signed<T>::value
+	constexpr Vector operator- () const
+	{ return Vector(-x); }
+
+	// additional methods
+	T getLength() const
+	{ return std::abs(x); }
+
+	WideType getLengthSquared() const
+	{ return std::pow(x, 2); }
+
+	bool hasNan() const { return std::isnan(x); }
+
+	bool hasInf() const { return std::isinf(x); }
+
+#ifndef __DOXYGEN__
+	IMPLEMENT_VECTOR_ACCESSOR2(x,x)
+	IMPLEMENT_VECTOR_ACCESSOR3(x,x,x)
+	IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,x)
+#endif
+
+	// depricated methods
+	template<typename U>
+	[[deprecated("Use common constructor instead!")]]
+	Vector<U, 1>
+	convert() const { return {*this}; }
+
+protected:
+	template<typename U>
+	friend IOStream&
+	operator<<(IOStream& os, const Vector<U, 1>& c);
+};
+
+template<typename U, typename T>
+static inline Vector<T, 1> operator * (const U &lhs, const Vector<T, 1> &rhs)
+{ return rhs * lhs; }
+
+#if __has_include(<modm/io/iostream.hpp>)
+#include <modm/io/iostream.hpp>
+
+template<typename U>
+IOStream&
+operator<< (IOStream& os, const Vector<U, 1>& v) {
+	os << "x=" << v.x;
+	return os;
 }
+#endif
 
-#include "vector1_impl.hpp"
+using Vector1f = Vector<float, 1>;
+using Vector1i = Vector<int16_t, 1>;
+using Vector1u = Vector<uint16_t, 1>;
 
-#endif // MODM_VECTOR1_HPP
+} // namespace modm
diff --git a/src/modm/math/geometry/vector1_impl.hpp b/src/modm/math/geometry/vector1_impl.hpp
deleted file mode 100644
index 3a9045d78a..0000000000
--- a/src/modm/math/geometry/vector1_impl.hpp
+++ /dev/null
@@ -1,285 +0,0 @@
-/*
- * Copyright (c) 2011, Fabian Greif
- * Copyright (c) 2012, Georgi Grinshpun
- * Copyright (c) 2012, Niklas Hauser
- *
- * This file is part of the modm project.
- *
- * This Source Code Form is subject to the terms of the Mozilla Public
- * License, v. 2.0. If a copy of the MPL was not distributed with this
- * file, You can obtain one at http://mozilla.org/MPL/2.0/.
- */
-// ----------------------------------------------------------------------------
-
-#ifndef MODM_VECTOR1_HPP
-	#error	"Don't include this file directly, use 'vector1.hpp' instead!"
-#endif
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 1>::Vector() :
-	x()
-{
-}
-
-template<typename T>
-modm::Vector<T, 1>::Vector(T inX) :
-	x(inX)
-{
-}
-
-template<typename T>
-modm::Vector<T, 1>::Vector(const modm::Matrix<T, 1, 1> &rhs) :
-	x(*reinterpret_cast<const T*>(&rhs))
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-void
-modm::Vector<T, 1>::set(const T& value)
-{
-	this->x = value;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-void
-modm::Vector<T, 1>::setX(const T& value)
-{
-	this->x = value;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-const T&
-modm::Vector<T, 1>::getX() const
-{
-	return this->x;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 1>&
-modm::Vector<T, 1>::operator = (const modm::Matrix<T, 1, 1> &rhs)
-{
-	x = *reinterpret_cast<const T*>(&rhs);
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 1>::operator == (const modm::Vector<T, 1> &rhs) const
-{
-	return (rhs.x == x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 1>::operator != (const modm::Vector<T, 1> &rhs) const
-{
-	return (rhs.x != x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 1>::operator < (const modm::Vector<T, 1> &rhs) const
-{
-	return (x < rhs.x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 1>::operator <= (const modm::Vector<T, 1> &rhs) const
-{
-	return (x <= rhs.x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 1>::operator > (const modm::Vector<T, 1> &rhs) const
-{
-	return (x > rhs.x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 1>::operator >= (const modm::Vector<T, 1> &rhs) const
-{
-	return (x >= rhs.x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-const T&
-modm::Vector<T, 1>::operator [] (uint8_t index) const
-{
-	return reinterpret_cast<const T*>(this)[index];
-}
-
-template<typename T>
-T&
-modm::Vector<T, 1>::operator [] (uint8_t index)
-{
-	return reinterpret_cast<T*>(this)[index];
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-T*
-modm::Vector<T, 1>::ptr()
-{
-	return reinterpret_cast<T*>(this);
-}
-
-template<typename T>
-const T*
-modm::Vector<T, 1>::ptr() const
-{
-	return reinterpret_cast<const T*>(this);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 1>
-modm::Vector<T, 1>::operator + (const modm::Vector<T, 1> &rhs) const
-{
-	return modm::Vector<T, 1>(x+rhs.x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 1>
-modm::Vector<T, 1>::operator - (const modm::Vector<T, 1> &rhs) const
-{
-	return modm::Vector<T, 1>(x-rhs.x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-T
-modm::Vector<T, 1>::operator * (const modm::Vector<T, 1> &rhs) const
-{
-	return x*rhs.x;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 1>
-modm::Vector<T, 1>::operator * (const T &rhs) const
-{
-	return modm::Vector<T, 1>(x*rhs);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 1>
-modm::Vector<T, 1>::operator / (const T &rhs) const
-{
-	return modm::Vector<T, 1>(x/rhs);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 1>&
-modm::Vector<T, 1>::operator += (const modm::Vector<T, 1> &rhs)
-{
-	x += rhs.x;
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 1>&
-modm::Vector<T, 1>::operator -= (const modm::Vector<T, 1> &rhs)
-{
-	x -= rhs.x;
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 1>&
-modm::Vector<T, 1>::operator *= (const T &rhs)
-{
-	x *= rhs;
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 1>&
-modm::Vector<T, 1>::operator /= (const T &rhs)
-{
-	x /= rhs;
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 1>
-modm::Vector<T, 1>::operator - () const
-{
-	return Vector<T, 1>(-x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-T
-modm::Vector<T, 1>::getLength() const
-{
-	return std::abs(x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-T
-modm::Vector<T, 1>::getLengthSquared() const
-{
-	return x * x;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Matrix<T, 1, 1>&
-modm::Vector<T, 1>::asMatrix()
-{
-	return *(modm::Matrix<T, 1, 1>*) this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-const modm::Matrix<T, 1, 1>&
-modm::Vector<T, 1>::asMatrix() const
-{
-	return *(modm::Matrix<T, 1, 1>*) this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 1>::hasNan() const
-{
-	return std::isnan(x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 1>::hasInf() const
-{
-	return std::isinf(x);
-}
-
-// ----------------------------------------------------------------------------
-//template<typename U, typename T>
-//static modm::Vector<T, 1> operator * (const U &lhs, const modm::Vector<T, 1> &rhs)
-//{
-//	return rhs * lhs;
-//}
diff --git a/src/modm/math/geometry/vector2.cpp b/src/modm/math/geometry/vector2.cpp
deleted file mode 100644
index 64bd297920..0000000000
--- a/src/modm/math/geometry/vector2.cpp
+++ /dev/null
@@ -1,75 +0,0 @@
-/*
- * Copyright (c) 2010-2012, Fabian Greif
- * Copyright (c) 2010, 2012, Martin Rosekeit
- * Copyright (c) 2012, Georgi Grinshpun
- * Copyright (c) 2012, Niklas Hauser
- *
- * This file is part of the modm project.
- *
- * This Source Code Form is subject to the terms of the Mozilla Public
- * License, v. 2.0. If a copy of the MPL was not distributed with this
- * file, You can obtain one at http://mozilla.org/MPL/2.0/.
- */
-// ----------------------------------------------------------------------------
-
-#include <modm/math/utils/operator.hpp>
-
-#include "vector2.hpp"
-
-// this explicit namespace is needed here, otherwise we get an error about
-// "specialization of ... in different namespace"
-namespace modm
-{
-	template<>
-	int16_t
-	Vector<int16_t, 2>::getLength() const
-	{
-		int32_t t;
-
-		t = math::mul(this->x, this->x);
-		t = math::mac(t, this-> y, this->y);
-
-		return math::sqrt(t);
-	}
-
-	template<>
-	int32_t
-	Vector<int16_t, 2>::getLengthSquared() const
-	{
-		int32_t t;
-
-		t = math::mul(this->x, this->x);
-		t = math::mac(t,this-> y, this->y);
-
-		return t;
-	}
-
-	template<>
-	int32_t
-	Vector<int16_t, 2>::dot(const modm::Vector<int16_t, 2>& other) const
-	{
-		int32_t t;
-
-		t = math::mul(this->x, other.x);
-		t = math::mac(t,this->y, other.y);
-
-		return t;
-	}
-
-	// ------------------------------------------------------------------------
-	template<>
-	template<>
-	Vector<double, 2>
-	Vector<float, 2>::convert() const
-	{
-		return Vector<double, 2>(this->x, this->y);
-	}
-
-	template<>
-	template<>
-	Vector<float, 2>
-	Vector<double, 2>::convert() const
-	{
-		return Vector<float, 2>(this->x, this->y);
-	}
-}
diff --git a/src/modm/math/geometry/vector2.hpp b/src/modm/math/geometry/vector2.hpp
index 7cb06624e9..8585c12c0c 100644
--- a/src/modm/math/geometry/vector2.hpp
+++ b/src/modm/math/geometry/vector2.hpp
@@ -3,6 +3,7 @@
  * Copyright (c) 2010, Martin Rosekeit
  * Copyright (c) 2012, Georgi Grinshpun
  * Copyright (c) 2012, Niklas Hauser
+ * Copyright (c) 2022, Thomas Sommer
  *
  * This file is part of the modm project.
  *
@@ -12,373 +13,409 @@
  */
 // ----------------------------------------------------------------------------
 
-#ifndef MODM_VECTOR2_HPP
-#define MODM_VECTOR2_HPP
+#pragma once
 
 #include <cmath>
-#include <stdint.h>
-#include <modm/io/iostream.hpp>
+#include <concepts>
 
-#include "geometric_traits.hpp"
 #include "angle.hpp"
 #include "vector.hpp"
 
 namespace modm
 {
-	// forward declaration
-	template <typename T>
-	class Location2D;
+// forward declaration
+template <typename T>
+class Location2D;
+
+/**
+ * \brief	Class for handling common vector operations (2D)
+ *
+ * Operations:
+ * \code
+ *	+ : addition of points
+	*	- : different of points
+	*	* : dot product or scalar multiplication
+	*	/ : scalar division
+	*	^ : cross product (determinant)
+	*	~ : perpendicular
+	* \endcode
+	*
+	* Adapted from the implementation of Gaspard Petit (gaspardpetit@gmail.com)
+	* and heavily modified.
+	*
+	* \see <a href"http://www-etud.iro.umontreal.ca/~petitg/cpp/point.html">Homepage</a>
+	*
+	* \author	Fabian Greif
+	* \author	Niklas Hauser
+	* \author	Thomas Sommer
+	* \ingroup	modm_math_geometry
+	*/
+template<typename T>
+class Vector<T, 2>
+{
+	using VecT1 = Vector<T, 1>;
+	using VecT2 = Vector;
+
+public:
+	using WideType = GeometricTraits<T>::WideType;
+	using FloatType = GeometricTraits<T>::FloatType;
+
+	T x{0}, y{0};
+
+	// fundamental constructors
+	constexpr Vector() = default;
+
+	constexpr explicit Vector(T xy)
+		: x(xy), y(xy) {}
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	constexpr explicit Vector(T xy)
+		: x(std::round(xy)), y(std::round(xy)) {}
+
+	constexpr Vector(T x, T y)
+		: x(x), y(y) {}
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	constexpr Vector(U x, U y)
+		: x(std::round(x)), y(std::round(y)) {}
+
+	template<typename U>
+	constexpr Vector(Vector<U, 2> v) : Vector(v.x, v.y) {}
+
+	// vector constructors
+	constexpr Vector(VecT1 vx, VecT1 vy) : Vector(vx.x, vy.x) {}
+	constexpr Vector(T x, VecT1 vy) : Vector(x, vy.x) {}
+	constexpr Vector(VecT1 vx, T y) : Vector(vx.x, y) {}
+
+	// matrix constructors
+	constexpr Vector(const Matrix<T, 2, 1> &rhs)
+		: Vector(reinterpret_cast<const T*>(&rhs)[0], reinterpret_cast<const T*>(&rhs)[1]) {}
+
+	// array constructors
+	template<typename U>
+	constexpr explicit Vector(U *array) : Vector(array[0], array[1]) {}
+
+	// as matrix convertors
+	Matrix<T, 2, 1>&
+	asMatrix() { return *((Matrix<T, 2, 1> *) this); }
+
+	const Matrix<T, 2, 1>&
+	asMatrix() const { return *((Matrix<T, 2, 1> *) this); }
+
+	Matrix<T, 1, 2>&
+	asTransposedMatrix() { return *((Matrix<T, 1, 2> *) this); }
+
+	const Matrix<T, 1, 2>&
+	asTransposedMatrix() const { return *((Matrix<T, 1, 2> *) this); }
+
+	// matrix assignment
+	Vector& operator= (const Matrix<T, 2, 1> &rhs) {
+		x = reinterpret_cast<const T*>(&rhs)[0];
+		y = reinterpret_cast<const T*>(&rhs)[1];
+
+		return *this;
+	}
+
+	// getters
+	T getX() const { return x; }
+	T getY() const { return y; }
+
+	// setters
+	void setX(T x) { this->x = x; }
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	void setX(U x) { this->x = std::round(x); }
+
+
+	void setY(T y) { this->y = y; }
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	void setY(U y) { this->x = std::round(y); }
+
+
+	void set(T x, T y) {
+		this->x = x;
+		this->y = y;
+	}
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	void set(U x, U y) {
+		this->x = std::round(x);
+		this->y = std::round(y);
+	}
+
+	// accessors
+	T& operator[] (std::size_t index)
+	{ return reinterpret_cast<T*>(this)[index]; }
+
+	const T& operator[] (std::size_t index) const
+	{ return reinterpret_cast<const T*>(this)[index]; }
+
+	T* ptr() { return reinterpret_cast<T*>(this); }
+
+	const T* ptr() const { return reinterpret_cast<const T*>(this); }
+
+	// operators
+	auto operator<=>(const Vector&) const = default;
+
+	constexpr Vector operator+ (const Vector &rhs) const
+	{ return Vector(x + rhs.x, y + rhs.y); }
+
+	constexpr Vector operator- (const Vector &rhs) const
+	{ return Vector(x - rhs.x, y - rhs.y); }
+
+	constexpr T operator* (const Vector &rhs) const
+	{ return x * rhs.x + y * rhs.y; }
+
+	constexpr T operator^ (const Vector &rhs) const
+	{ return x * rhs.y - y * rhs.x; }
+
+	template <typename U>
+	constexpr Vector operator* (U scale) const
+	{ return Vector<U, 2>(x * scale, y * scale); }
+
+	template <typename U>
+	constexpr Vector operator/ (U scale) const
+	{ return Vector<U, 2>(x / scale, y / scale); }
+
+	Vector& operator+= (const Vector &rhs)
+	{ x += rhs.x; y += rhs.y; return *this; }
+
+	Vector& operator-= (const Vector &rhs)
+	{ x -= rhs.x; y -= rhs.y; return *this; }
+
+	template<typename U>
+	Vector& operator*= (U scale)
+	{ x *= scale; y *= scale; return *this; }
+
+	template<typename U>
+	Vector& operator/= (U scale)
+	{ x /= scale; y /= scale; return *this; }
+
+	Vector& operator~ ()
+	{ *this = perpendicular(); return *this; }
+
+	// template <typename>
+	// requires std::is_signed<T>::value
+	constexpr Vector operator- () const
+	{ return Vector(-x, -y); }
+
+	// additional methods
+	T getLength() const
+	{ return std::sqrt(getLengthSquared()); }
+
+	T getLength() const
+	requires std::integral<T>
+	{ return round(std::sqrt(getLengthSquared())); }
+
+	WideType getLengthSquared() const
+	{ return std::pow(x, 2) + std::pow(y, 2); }
+
+	WideType getDistanceTo(const Vector& other) const
+	{ return (other - *this).getLength(); }
+
+	float getAngle() const
+	{ return std::atan2(y, x); }
+
+	float getAngleTo(const Vector& other) const
+	{ return (other - *this).getAngle(); }
+
+	/**
+	 * \brief	Normalize length to 1
+	 *
+	 * \warning	This method is only useful if T is a floating point type.
+	 * 			For integer types the result might be wrong!
+	 */
+	Vector& normalize() {
+		operator/=( getLength() );
+		return *this;
+	}
+
+	Vector normalized() const {
+		return *this / getLength();
+	}
+
+	/**
+	 * \brief	Scale the vector to \p length
+	 */
+	Vector& scale(float length) {
+		operator*=( length / getLength() );
+		return *this;
+	}
+
+	Vector scaled(float length) const {
+		return *this * (length / getLength());
+	}
+
+	// TODO implement as operator+=(Angle phi), operator-=(Angle phi)
+	Vector&
+	rotate(float phi) {
+		const float c = std::cos(phi);
+		const float s = std::sin(phi);
+		const Vector<float, 2> tmp(c * x - s * y, s * x + c * y);
+		set(tmp.x, tmp.y);
+
+		return *this;
+	}
 
 	/**
-	 * \brief	Class for handling common vector operations (2D)
+	 * \brief	Same like operator+= but also returns reference
+	 */
+	Vector&
+	translate(Vector v)
+	{ operator+=(v); return *this; }
+
+	/**
+	 * \brief	Calculate the dot-product
+	 *
+	 * Also known as the scalar product.
 	 *
-	 * Operations:
 	 * \code
-	 *	+ : addition of points
-	 *	- : different of points
-	 *	* : dot product or scalar multiplication
-	 *	/ : scalar division
-	 *	^ : cross product (determinant)
-	 *	~ : perpendicular
+	 * this.x * other.x + this.y * other.y
 	 * \endcode
+	 */
+	WideType dot(const Vector& other) const
+	{ return WideType(x) * WideType(other.x) + WideType(y) * WideType(other.y); }
+
+	/**
+	 * \brief	Calculate the cross-product
 	 *
-	 * Adapted from the implementation of Gaspard Petit (gaspardpetit@gmail.com)
-	 * and heavily modified.
+	 * In 2D there is no clear definition of this operation.
 	 *
-	 * \see <a href"http://www-etud.iro.umontreal.ca/~petitg/cpp/point.html">Homepage</a>
+	 * This implementation is the most common one and will return the
+	 * magnitude of the vector that would result from a regular
+	 * 3D cross product of the input vectors, taking their Z values
+	 * implicitly as 0 (i.e. treating the 2D space as a plane in the 3D space).
+	 * The 3D cross product will be perpendicular to that plane, and thus
+	 * have 0 X & Y components (thus the scalar returned is the Z value of
+	 * the 3D cross product vector).
 	 *
-	 * \author	Fabian Greif
-	 * \author	Niklas Hauser
+	 * \code
+	 * this.x * other.y - this.y * other.x
+	 * \endcode
 	 *
-	 * \ingroup	modm_math_geometry
-	 */
-	template<typename T>
-	class Vector<T, 2>
-	{
-		friend class Location2D<T>;
-
-	public:
-		typedef typename GeometricTraits<T>::WideType WideType;
-		typedef typename GeometricTraits<T>::FloatType FloatType;
-
-	public:
-		/**
-		 * \brief	Default-Constructor
-		 *
-		 * Creates a Vector with coordinates (0, 0).
-		 */
-		Vector();
-
-		Vector(const T& inX, const T& inY);
-
-		Vector(const Vector<T, 1> &inX, const Vector<T, 1> &inY);
-		Vector(const T &inX, const Vector<T, 1> &inY);
-		Vector(const Vector<T, 1> &inX, const T &inY);
-		explicit Vector(T inVal);
-		Vector(const Matrix<T, 2, 1> &rhs);
-
-		inline void
-		setX(const T& value);
-
-		inline void
-		setY(const T& value);
-
-		inline void
-		set(const T& x, const T& y);
-
-
-		inline const T&
-		getX() const;
-
-		inline const T&
-		getY() const;
-
-		/**
-		 * \brief	Calculate length of the vector
-		 */
-		T
-		getLength() const;
-
-		/**
-		 * \brief	Calculate squared length of the vector
-		 *
-		 * This method is considerably faster than getLength() because it
-		 * doesn't need to calculate the square root.
-		 *
-		 * \return	squared length (x*x + y*y)
-		 */
-		WideType
-		getLengthSquared() const;
-
-		/**
-		 * \brief	Calculate the absolute angle
-		 *
-		 * \code
-		 * atan2(y, x)
-		 * \endcode
-		 */
-		float
-		getAngle() const;
-
-		/**
-		 * \brief	Normalize length to 1
-		 *
-		 * \warning	This method is only useful if T is a floating point type.
-		 * 			For integer types the result might be wrong!
-		 */
-		Vector&
-		normalize();
-
-		Vector
-		normalized() const;
-
-		/**
-		 * \brief	Scale the vector to \p length
-		 */
-		Vector&
-		scale(float length);
-
-		Vector
-		scaled(float length) const;
-
-
-		Vector&
-		rotate(float phi);
-
-		/**
-		 * \brief	Move the point in x and y direction
-		 */
-		Vector&
-		translate(const Vector& vector);
-
-
-		WideType
-		getDistanceTo(const Vector& other) const;
-
-		float
-		getAngleTo(const Vector& other) const;
-
-		/**
-		 * \brief	Calculate the dot-product
-		 *
-		 * Also known as the scalar product.
-		 *
-		 * \code
-		 * this.x * other.x + this.y * other.y
-		 * \endcode
-		 */
-		WideType
-		dot(const Vector& other) const;
-
-		/**
-		 * \brief	Calculate the cross-product
-		 *
-		 * In 2D there is no clear definition of this operation.
-		 *
-		 * This implementation is the most common one and will return the
-		 * magnitude of the vector that would result from a regular
-		 * 3D cross product of the input vectors, taking their Z values
-		 * implicitly as 0 (i.e. treating the 2D space as a plane in the 3D space).
-		 * The 3D cross product will be perpendicular to that plane, and thus
-		 * have 0 X & Y components (thus the scalar returned is the Z value of
-		 * the 3D cross product vector).
-		 *
-		 * \code
-		 * this.x * other.y - this.y * other.x
-		 * \endcode
-		 *
-		 * Other implementations take no arguments and returns a vector
-		 * perpendicular to the input vector. This can be reached with the
-		 * toOrthogonalVector() method, which returns a perpendicular copy
-		 * of the vector.
-		 */
-		WideType
-		cross(const Vector& other) const;
-
-		/**
-		 * \brief	Convert between Point-objects with different base-types
-		 */
-		template<typename U>
-		Vector<U, 2>
-		convert() const;
-
-		/**
-		 * \brief	Returns a perpendicular copy of the vector
-		 *
-		 * \return	(y, -x)
-		 */
-		Vector
-		toOrthogonalVector() const;
-
-		// TODO
-		Vector
-		perpendicular() const;
-
-		/**
-		 * \brief	Check if three points are in a counter-clock wise direction
-		 *
-		 * Check if we move counter-clock wise if we move from the first point
-		 * to the second and the third.
-		 *
-		 * If all three points are in a line there are three possibilities:
-		 *  1) strait line: third point behind the second (returns 1)
-		 *  2) last point between the other two (returns 0)
-		 *  3) third point before the first one (returns -1)
-		 *
-		 * This definition is useful for inclusion or intersection testing.
-		 */
-		static int_fast8_t
-		ccw(const Vector& a, const Vector& b, const Vector& c);
-
-		Vector& operator = (const Matrix<T, 2, 1> &rhs);
-
-		bool operator == (const Vector &rhs) const;
-		bool operator != (const Vector &rhs) const;
-		bool operator < (const Vector &rhs) const;
-		bool operator <= (const Vector &rhs) const;
-		bool operator > (const Vector &rhs) const;
-		bool operator >= (const Vector &rhs) const;
-
-		const T& operator [] (uint8_t index) const;
-		T& operator [] (uint8_t index);
-
-		T* ptr();
-		const T* ptr() const;
-
-		Vector operator - () const;
-		Vector operator - (const Vector &rhs) const;
-		Vector operator + (const Vector &rhs) const;
-		T operator * (const Vector &rhs) const;
-		T operator ^ (const Vector &rhs) const;
-		Vector operator * (float rhs) const;
-		Vector operator / (float rhs) const;
-
-		Vector& operator += (const Vector &rhs);
-		Vector& operator -= (const Vector &rhs);
-		Vector& operator *= (const T &rhs);
-		Vector& operator /= (const T &rhs);
-		Vector& operator ~ ();
-
-		Matrix<T, 2, 1>&
-		asMatrix();
-
-		const Matrix<T, 2, 1>&
-		asMatrix() const;
-
-		Matrix<T, 1, 2>&
-		asTransposedMatrix();
-
-		const Matrix<T, 1, 2>&
-		asTransposedMatrix() const;
-
-		bool hasNan() const;
-		bool hasInf() const;
-
-	#ifndef __DOXYGEN__
-		IMPLEMENT_VECTOR_ACCESSOR2(x,x); IMPLEMENT_VECTOR_ACCESSOR2(x,y);
-		IMPLEMENT_VECTOR_ACCESSOR2(y,x); IMPLEMENT_VECTOR_ACCESSOR2(y,y);
-
-		IMPLEMENT_VECTOR_ACCESSOR3(x,x,x); IMPLEMENT_VECTOR_ACCESSOR3(x,x,y);
-		IMPLEMENT_VECTOR_ACCESSOR3(x,y,x); IMPLEMENT_VECTOR_ACCESSOR3(x,y,y);
-		IMPLEMENT_VECTOR_ACCESSOR3(y,x,x); IMPLEMENT_VECTOR_ACCESSOR3(y,x,y);
-		IMPLEMENT_VECTOR_ACCESSOR3(y,y,x); IMPLEMENT_VECTOR_ACCESSOR3(y,y,y);
-
-		IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,x); IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,y);
-		IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,x); IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,y);
-		IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,x); IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,y);
-		IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,x); IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,y);
-
-		IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,x); IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,y);
-		IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,x); IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,y);
-		IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,x); IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,y);
-		IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,x); IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,y);
-	#endif
-
-	public:
-		T x;
-		T y;
-
-	protected:
-		template<typename U>
-		friend IOStream&
-		operator <<(IOStream& os, const Vector<U, 2>& c);
-
-		template<typename U>
-		friend Vector<U, 2>
-		operator * (float scale, const Vector<U, 2> &vector);
-	};
-
-	typedef Vector<float, 2> 		Vector2f;
-	typedef Vector<int16_t, 2> 		Vector2i;
-	typedef Vector<uint16_t, 2> 	Vector2u;
-
-	// ------------------------------------------------------------------------
-	// Global functions
-	// ------------------------------------------------------------------------
-	/**
-	 * \brief	Stream operator for \b modm::Vector<U, 2>
-	 * \ingroup	modm_math_geometry
+	 * Other implementations take no arguments and returns a vector
+	 * perpendicular to the input vector. This can be reached with the
+	 * toOrthogonalVector() method, which returns a perpendicular copy
+	 * of the vector.
 	 */
-	template<typename U>
-	IOStream&
-	operator << (IOStream& os, const Vector<U, 2>& c);
+	WideType cross(const Vector& other) const
+	{ return WideType(x) * WideType(other.y) - WideType(y) * WideType(other.x); }
 
-	/**
-	 * \brief	Scalar multiplication
-	 * \ingroup	modm_math_geometry
-	 */
-	template<typename U>
-	Vector<U, 2>
-	operator * (float scale, const Vector<U, 2> &vector);
+	Vector toOrthogonalVector() const
+	{ return Vector(y, -x); }
+
+	Vector perpendicular() const
+	{ return Vector(y, -x); }
 
 	/**
-	 * \brief	Scalar division
-	 * \ingroup	modm_math_geometry
+	 * \brief	Check if three points are in a counter-clock wise direction
+	 *
+	 * Check if we move counter-clock wise if we move from the first point
+	 * to the second and the third.
+	 *
+	 * If all three points are in a line there are three possibilities:
+	 *  1) strait line: third point behind the second (returns 1)
+	 *  2) last point between the other two (returns 0)
+	 *  3) third point before the first one (returns -1)
+	 *
+	 * This definition is useful for inclusion or intersection testing.
 	 */
-	template<typename U>
-	Vector<U, 2>
-	operator / (float scale, const Vector<U, 2> &vector);
-
-	// ------------------------------------------------------------------------
-	// Declaration of specialized methods
-	// ------------------------------------------------------------------------
-	template<>
-	int16_t
-	Vector<int16_t, 2>::getLength() const;
+	static int8_t
+	ccw(const Vector& a, const Vector& b, const Vector& c) {
+		const Vector<WideType, 2> v1 = b - a;
+		const Vector<WideType, 2> v2 = c - a;
+		const WideType d1 = v1.x * v2.y;
+		const WideType d2 = v1.y * v2.x;
+
+		if (d1 > d2)
+			return 1;
+		else if (d1 < d2)
+			return -1;
+		else
+		{
+			if ((v1.x * v2.x < 0) || (v1.y * v2.y < 0))
+				return -1;
+			else
+				return (v1.x * v1.x + v1.y * v1.y) < (v2.x * v2.x + v2.y * v2.y) ? 1 : 0;
+		}
+	}
 
-	template<>
-	int32_t
-	Vector<int16_t, 2>::getLengthSquared() const;
+	bool hasNan() const { return std::isnan(x) || std::isnan(y); }
+	bool hasInf() const { return std::isinf(x) || std::isinf(y); }
 
-	template<>
-	int32_t
-	Vector<int16_t, 2>::dot(const modm::Vector<int16_t, 2>& other) const;
+#ifndef __DOXYGEN__
+	IMPLEMENT_VECTOR_ACCESSOR2(x,x); IMPLEMENT_VECTOR_ACCESSOR2(x,y);
+	IMPLEMENT_VECTOR_ACCESSOR2(y,x); IMPLEMENT_VECTOR_ACCESSOR2(y,y);
 
+	IMPLEMENT_VECTOR_ACCESSOR3(x,x,x); IMPLEMENT_VECTOR_ACCESSOR3(x,x,y);
+	IMPLEMENT_VECTOR_ACCESSOR3(x,y,x); IMPLEMENT_VECTOR_ACCESSOR3(x,y,y);
+	IMPLEMENT_VECTOR_ACCESSOR3(y,x,x); IMPLEMENT_VECTOR_ACCESSOR3(y,x,y);
+	IMPLEMENT_VECTOR_ACCESSOR3(y,y,x); IMPLEMENT_VECTOR_ACCESSOR3(y,y,y);
 
-	template<> template<>
-	Vector<double, 2>
-	Vector<float, 2>::convert() const;
+	IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,x); IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,y);
+	IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,x); IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,y);
+	IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,x); IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,y);
+	IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,x); IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,y);
 
-	template<> template<>
-	Vector<float, 2>
-	Vector<double, 2>::convert() const;
+	IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,x); IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,y);
+	IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,x); IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,y);
+	IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,x); IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,y);
+	IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,x); IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,y);
+#endif
 
-	// round for everything that's not float => double or double => float
-	template<> template<typename U>
+	// depricated methods
+	template<typename U>
+	[[deprecated("Use common constructor instead!")]]
 	Vector<U, 2>
-	Vector<float, 2>::convert() const
-	{
-		return Vector<U, 2>(round(this->x), round(this->y));
-	}
+	convert() const { return {*this}; }
 
-	template<> template<typename U>
-	Vector<U, 2>
-	Vector<double, 2>::convert() const
-	{
-		return Vector<U, 2>(round(this->x), round(this->y));
-	}
+protected:
+	template<typename U>
+	friend Vector<U, 2>
+	operator* (float scale, const Vector<U, 2> &vector);
+
+	template<typename U>
+	friend IOStream&
+	operator<<(IOStream& os, const Vector<U, 2>& c);
+};
+
+/**
+ * \brief	Scalar multiplication
+ * \ingroup	modm_math_geometry
+ */
+template<typename U>
+Vector<U, 2>
+operator* (float scale, const Vector<U, 2> &vector)
+{ return vector * scale; }
+
+/**
+ * \brief	Scalar division
+ * \ingroup	modm_math_geometry
+ */
+template<typename U>
+Vector<U, 2>
+operator/ (float scale, const Vector<U, 2> &vector)
+{ return vector / scale; }
+
+#if __has_include(<modm/io/iostream.hpp>)
+#include <modm/io/iostream.hpp>
+
+template<typename U>
+IOStream&
+operator<< (IOStream& os, const Vector<U, 2>& v) {
+	os << "x=" << v.x << "\ty=" << v.y;
+	return os;
 }
+#endif
 
-#include "vector2_impl.hpp"
+using Vector2f = Vector<float, 2>;
+using Vector2i = Vector<int16_t, 2>;
+using Vector2u = Vector<uint16_t, 2>;
 
-#endif // MODM_VECTOR2_HPP
+} // namespace modm
\ No newline at end of file
diff --git a/src/modm/math/geometry/vector2_impl.hpp b/src/modm/math/geometry/vector2_impl.hpp
deleted file mode 100644
index b30f16be8e..0000000000
--- a/src/modm/math/geometry/vector2_impl.hpp
+++ /dev/null
@@ -1,579 +0,0 @@
-/*
- * Copyright (c) 2011, Fabian Greif
- * Copyright (c) 2012, Niklas Hauser
- *
- * This file is part of the modm project.
- *
- * This Source Code Form is subject to the terms of the Mozilla Public
- * License, v. 2.0. If a copy of the MPL was not distributed with this
- * file, You can obtain one at http://mozilla.org/MPL/2.0/.
- */
-// ----------------------------------------------------------------------------
-
-#ifndef MODM_VECTOR2_HPP
-	#error	"Don't include this file directly, use 'vector2.hpp' instead!"
-#endif
-
-#include <cmath>
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>::Vector() :
-	x(),
-	y()
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>::Vector(const T& inX, const T& inY) :
-	x(inX),
-	y(inY)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>::Vector(
-		const modm::Vector<T, 1> &inX,
-		const modm::Vector<T, 1> &inY) :
-	x(inX.x),
-	y(inY.x)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>::Vector(const T &inX, const modm::Vector<T, 1> &inY) :
-	x(inX),
-	y(inY.x)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>::Vector(const modm::Vector<T, 1> &inX, const T &inY) :
-	x(inX.x),
-	y(inY)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>::Vector(T inVal) :
-	x(inVal),
-	y(inVal)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>::Vector(const modm::Matrix<T, 2, 1> &rhs) :
-	x(reinterpret_cast<const T*>(&rhs)[0]),
-	y(reinterpret_cast<const T*>(&rhs)[1])
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-void
-modm::Vector<T, 2>::set(const T& newX, const T& newY)
-{
-	this->x = newX;
-	this->y = newY;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-void
-modm::Vector<T, 2>::setX(const T& value)
-{
-	this->x = value;
-}
-
-template<typename T>
-void
-modm::Vector<T, 2>::setY(const T& value)
-{
-	this->y = value;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-const T&
-modm::Vector<T, 2>::getX() const
-{
-	return this->x;
-}
-
-template<typename T>
-const T&
-modm::Vector<T, 2>::getY() const
-{
-	return this->y;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-T
-modm::Vector<T, 2>::getLength() const
-{
-	float tx = this->x;
-	float ty = this->y;
-
-	return GeometricTraits<T>::round(std::sqrt(tx*tx + ty*ty));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-typename modm::Vector<T, 2>::WideType
-modm::Vector<T, 2>::getLengthSquared() const
-{
-	WideType tx = static_cast<WideType>(this->x);
-	WideType ty = static_cast<WideType>(this->y);
-
-	return tx * tx + ty * ty;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-float
-modm::Vector<T, 2>::getAngle() const
-{
-	return std::atan2(this->y, this->x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>&
-modm::Vector<T, 2>::normalize()
-{
-	T length = this->getLength();
-
-	this->x = this->x / length;
-	this->y = this->y / length;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>
-modm::Vector<T, 2>::normalized() const
-{
-	Vector a(*this);
-	a.normalize();
-	return a;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>&
-modm::Vector<T, 2>::scale(float length)
-{
-	float factor = length / getLength();
-
-	this->x = this->x * factor;
-	this->y = this->y * factor;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>
-modm::Vector<T, 2>::scaled(float length) const
-{
-	Vector a(*this);
-	a.scale(length);
-	return a;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>&
-modm::Vector<T, 2>::rotate(float phi)
-{
-	float c = std::cos(phi);
-	float s = std::sin(phi);
-
-	// without rounding the result might be false for T = integer
-	T tx =    GeometricTraits<T>::round(c * this->x - s * this->y);
-	this->y = GeometricTraits<T>::round(s * this->x + c * this->y);
-	this->x = tx;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>&
-modm::Vector<T, 2>::translate(const Vector<T, 2>& vector)
-{
-	this->x += vector.x;
-	this->y += vector.y;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-typename modm::Vector<T, 2>::WideType
-modm::Vector<T, 2>::getDistanceTo(const Vector<T, 2>& other) const
-{
-	return (other - *this).getLength();
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-float
-modm::Vector<T, 2>::getAngleTo(const Vector<T, 2>& other) const
-{
-	return (other - *this).getAngle();
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-typename modm::Vector<T, 2>::WideType
-modm::Vector<T, 2>::dot(const modm::Vector<T, 2>& other) const
-{
-	return (static_cast<WideType>(this->x) * static_cast<WideType>(other.x) +
-			static_cast<WideType>(this->y) * static_cast<WideType>(other.y));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-typename modm::Vector<T, 2>::WideType
-modm::Vector<T, 2>::cross(const modm::Vector<T, 2>& other) const
-{
-	return (static_cast<WideType>(this->x) * static_cast<WideType>(other.y) -
-			static_cast<WideType>(this->y) * static_cast<WideType>(other.x));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T> template<typename U>
-modm::Vector<U, 2>
-modm::Vector<T, 2>::convert() const
-{
-	return Vector<U, 2>(static_cast<U>(this->x), static_cast<U>(this->y));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>
-modm::Vector<T, 2>::toOrthogonalVector() const
-{
-	return modm::Vector<T, 2>(y, -x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>
-modm::Vector<T, 2>::perpendicular() const
-{
-	return modm::Vector<T, 2>(y, -x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-int_fast8_t
-modm::Vector<T, 2>::ccw(const Vector& a, const Vector& b,
-		const Vector& c)
-{
-	WideType dx1 = b.x - a.x;
-	WideType dy1 = b.y - a.y;
-	WideType dx2 = c.x - a.x;
-	WideType dy2 = c.y - a.y;
-
-	WideType d1 = dx1 * dy2;
-	WideType d2 = dy1 * dx2;
-
-	if (d1 > d2) {
-		return 1;
-	}
-	else if (d1 < d2) {
-		return -1;
-	}
-	else
-	{
-		if ((dx1 * dx2 < 0) || (dy1 * dy2 < 0)) {
-			return -1;
-		}
-		else
-		{
-			if ((dx1 * dx1 + dy1 * dy1) >= (dx2 * dx2 + dy2 * dy2)) {
-				return 0;
-			}
-			else {
-				return 1;
-			}
-		}
-	}
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>& modm::Vector<T, 2>::operator = (const modm::Matrix<T, 2, 1> &rhs)
-{
-	x = reinterpret_cast<const T*>(&rhs)[0];
-	y = reinterpret_cast<const T*>(&rhs)[1];
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool modm::Vector<T, 2>::operator == (const modm::Vector<T, 2> &rhs) const
-{
-	return ((rhs.x == x) && (rhs.y == y));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool modm::Vector<T, 2>::operator != (const modm::Vector<T, 2> &rhs) const
-{
-	return ((rhs.x != x) || (rhs.y != y));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool modm::Vector<T, 2>::operator < (const modm::Vector<T, 2> &rhs) const
-{
-	return (x < rhs.x) || ((x == rhs.x) && (y < rhs.y));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool modm::Vector<T, 2>::operator <= (const modm::Vector<T, 2> &rhs) const
-{
-	return (x < rhs.x) || ((x == rhs.x) && (y <= rhs.y));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 2>::operator > (const modm::Vector<T, 2> &rhs) const
-{
-	return (x > rhs.x) || ((x == rhs.x) && (y > rhs.y));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 2>::operator >= (const modm::Vector<T, 2> &rhs) const
-{
-	return (x > rhs.x) || ((x == rhs.x) && (y >= rhs.y));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-const T&
-modm::Vector<T, 2>::operator [] (uint8_t index) const
-{
-	return reinterpret_cast<const T*>(this)[index];
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-T&
-modm::Vector<T, 2>::operator [] (uint8_t index)
-{
-	return reinterpret_cast<T*>(this)[index];
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-T*
-modm::Vector<T, 2>::ptr()
-{
-	return reinterpret_cast<T*>(this);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-const T*
-modm::Vector<T, 2>::ptr() const
-{
-	return reinterpret_cast<const T*>(this);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>
-modm::Vector<T, 2>::operator - () const
-{
-	return Vector<T, 2>(-x, -y);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>
-modm::Vector<T, 2>::operator - (const modm::Vector<T, 2> &rhs) const
-{
-	return modm::Vector<T, 2>(x - rhs.x, y - rhs.y);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>
-modm::Vector<T, 2>::operator + (const modm::Vector<T, 2> &rhs) const
-{
-	return modm::Vector<T, 2>(x + rhs.x, y + rhs.y);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-T
-modm::Vector<T, 2>::operator * (const modm::Vector<T, 2> &rhs) const
-{
-	return (x * rhs.x + y * rhs.y);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-T
-modm::Vector<T, 2>::operator ^ (const modm::Vector<T, 2> &rhs) const
-{
-	return (x * rhs.y - y * rhs.x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>
-modm::Vector<T, 2>::operator * (float scale) const
-{
-	return Vector<T, 2>(GeometricTraits<T>::round(this->x * scale),
-						GeometricTraits<T>::round(this->y * scale));
-}
-
-template<typename T>
-modm::Vector<T, 2>
-modm::Vector<T, 2>::operator / (float scale) const
-{
-	return Vector<T, 2>(GeometricTraits<T>::round(this->x / scale),
-						GeometricTraits<T>::round(this->y / scale));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>&
-modm::Vector<T, 2>::operator += (const modm::Vector<T, 2> &rhs)
-{
-	x += rhs.x;
-	y += rhs.y;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>&
-modm::Vector<T, 2>::operator -= (const modm::Vector<T, 2> &rhs)
-{
-	x -= rhs.x;
-	y -= rhs.y;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>&
-modm::Vector<T, 2>::operator *= (const T &rhs)
-{
-	x *= rhs;
-	y *= rhs;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>&
-modm::Vector<T, 2>::operator /= (const T &rhs)
-{
-	x /= rhs;
-	y /= rhs;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 2>&
-modm::Vector<T, 2>::operator ~ ()
-{
-	*this = perpendicular();
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Matrix<T, 2, 1>&
-modm::Vector<T, 2>::asMatrix()
-{
-	return *((modm::Matrix<T, 2, 1> *) this);
-}
-
-template<typename T>
-const modm::Matrix<T, 2, 1>&
-modm::Vector<T, 2>::asMatrix() const
-{
-	return *((modm::Matrix<T, 2, 1> *) this);
-}
-//-----------------------------------------------------------------------------
-template<typename T>
-modm::Matrix<T, 1, 2>&
-modm::Vector<T, 2>::asTransposedMatrix()
-{
-	return *((modm::Matrix<T, 1, 2> *) this);
-}
-
-template<typename T>
-const modm::Matrix<T, 1, 2>&
-modm::Vector<T, 2>::asTransposedMatrix() const
-{
-	return *((modm::Matrix<T, 1, 2> *) this);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool modm::Vector<T, 2>::hasNan() const
-{
-	return std::isnan(x) || std::isnan(y);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool modm::Vector<T, 2>::hasInf() const
-{
-	return std::isinf(x) || std::isinf(y);
-}
-
-// ----------------------------------------------------------------------------
-// Global functions
-// ----------------------------------------------------------------------------
-template <typename U>
-modm::IOStream&
-modm::operator << (modm::IOStream& s, const modm::Vector<U, 2>& c)
-{
-	s << "x=" << c.x << ", y=" << c.y;
-	return s;
-}
-
-// ----------------------------------------------------------------------------
-template<typename U>
-modm::Vector<U, 2>
-modm::operator * (float scale, const Vector<U, 2> &vector)
-{
-	// invoke member function
-	return vector * scale;
-}
-
-template<typename U>
-modm::Vector<U, 2>
-modm::operator / (float scale, const Vector<U, 2> &vector)
-{
-	// invoke member function
-	return vector / scale;
-}
-
diff --git a/src/modm/math/geometry/vector3.hpp b/src/modm/math/geometry/vector3.hpp
index 6287ee671b..2003fa6bb9 100644
--- a/src/modm/math/geometry/vector3.hpp
+++ b/src/modm/math/geometry/vector3.hpp
@@ -2,6 +2,7 @@
  * Copyright (c) 2011-2012, Fabian Greif
  * Copyright (c) 2012, Georgi Grinshpun
  * Copyright (c) 2012, Niklas Hauser
+ * Copyright (c) 2022, Thomas Sommer
  *
  * This file is part of the modm project.
  *
@@ -11,208 +12,333 @@
  */
 // ----------------------------------------------------------------------------
 
-#ifndef MODM_VECTOR3_HPP
-#define MODM_VECTOR3_HPP
+#pragma once
 
-#include <stdint.h>
+#include <concepts>
 #include "vector.hpp"
 
 namespace modm
 {
-	/**
-	 * \brief	Class for handling common vector operations (3D)
-	 *
-	 *	+ : addition of points
-	 *	- : different of points
-	 *	* : dot product or scalar multiplication
-	 *	/ : scalar division
-	 *	^ : cross product (determinant)
-	 *
-	 * Adapted from the implementation of Gaspard Petit (gaspardpetit@gmail.com).
-	 *
-	 * \see <a href"http://www-etud.iro.umontreal.ca/~petitg/cpp/point.html">Homepage</a>
-	 *
-	 * \ingroup	modm_math_geometry
-	 * \author	Niklas Hauser
-	 */
-	template<typename T>
-	class Vector<T, 3>
-	{
-	public:
-		Vector();
+/**
+ * \brief	Class for handling common vector operations (3D)
+ *
+ *	+ : addition of points
+	*	- : different of points
+	*	* : dot product or scalar multiplication
+	*	/ : scalar division
+	*	^ : cross product (determinant)
+	*
+	* Adapted from the implementation of Gaspard Petit (gaspardpetit@gmail.com).
+	*
+	* \see <a href"http://www-etud.iro.umontreal.ca/~petitg/cpp/point.html">Homepage</a>
+	*
+	* \author	Niklas Hauser
+	* \author	Thomas Sommer
+	* \ingroup	modm_math_geometry
+	*/
+template<typename T>
+class Vector<T, 3>
+{
+	using VecT1 = Vector<T, 1>;
+	using VecT2 = Vector<T, 2>;
+	using VecT3 = Vector;
+
+public:
+	using WideType = GeometricTraits<T>::WideType;
+	using WideWideType = GeometricTraits<WideType>::WideType;
+	using FloatType = GeometricTraits<T>::FloatType;
+
+	T x{0}, y{0}, z{0};
+
+	// fundamental constructors
+	constexpr Vector() =  default;
+
+	constexpr explicit Vector(T xyz)
+		: x(xyz), y(xyz), z(xyz) {}
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	constexpr explicit Vector(T xyz)
+		: x(std::round(xyz)), y(std::round(xyz)), z(std::round(xyz)) {}
+
+	constexpr Vector(T x, T y, T z)
+		: x(x), y(y), z(z) {}
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	constexpr Vector(U x, U y, U z)
+		: x(std::round(x)), y(std::round(y)), z(std::round(z)) {}
+
+	template<typename U>
+	constexpr Vector(const Vector<U, 3> &v) : Vector(v.x, v.y, v.z) {}
+
+	// vector constructors
+	constexpr Vector(VecT1 vx, T y, T z) : Vector(vx.x, y, z) {}
+	constexpr Vector(T x, VecT1 vy, T z) : Vector(x, vy.x, z) {}
+	constexpr Vector(T x, T y, VecT1 vz) : Vector(x, y, vz.x) {}
+
+	constexpr Vector(VecT1 vx, T y, VecT1 vz) : Vector(vx.x, y, vz.x) {}
+	constexpr Vector(VecT1 vx, VecT1 vy, T z) : Vector(vx.x, vy.x, z) {}
+	constexpr Vector(T x, VecT1 vy, VecT1 vz) : Vector(x, vy.x, vz.x) {}
+
+	constexpr Vector(VecT1 vx, VecT1 vy, VecT1 vz) : Vector(vx.x, vy.x, vz.x) {}
+
+	constexpr Vector(Vector<T,2> vxy, T z) : Vector(vxy.x, vxy.y, z) {}
+	constexpr Vector(T x, VecT2 vyz) : Vector(x, vyz.x, vyz.y) {}
+
+	constexpr Vector(VecT2 vxy, VecT1 vz) : Vector(vxy.x, vxy.y, vz.x) {}
+	constexpr Vector(VecT1 vx, VecT2 vyz) : Vector(vx.x, vyz.x, vyz.y) {}
+
+	// matrix constructors
+	constexpr Vector(const Matrix<T, 3, 1> &rhs)
+		: x(reinterpret_cast<const T*>(&rhs)[0]),
+		y(reinterpret_cast<const T*>(&rhs)[1]),
+		z(reinterpret_cast<const T*>(&rhs)[2])
+	{}
+
+	// array constructors
+	template<typename U>
+	constexpr explicit Vector(U *array) : Vector(array[0], array[1], array[2]) {}
+
+	// as matrix convertors
+	Matrix<T, 3, 1>&
+	asMatrix() { return *(Matrix<T, 3, 1>*)this; }
+
+	const Matrix<T, 3, 1>&
+	asMatrix() const { return *(Matrix<T, 3, 1>*)this; }
+
+	Matrix<T, 1, 3>&
+	asTransposedMatrix() { return *(Matrix<T, 1, 3>*)this; }
+
+	const Matrix<T, 1, 3>&
+	asTransposedMatrix() const { return *(Matrix<T, 1, 3>*)this; }
+
+	// matrix assignment
+	Vector& operator= (const Matrix<T, 3, 1> &rhs) {
+		x = reinterpret_cast<const T*>(&rhs)[0];
+		y = reinterpret_cast<const T*>(&rhs)[1];
+		z = reinterpret_cast<const T*>(&rhs)[2];
+
+		return *this;
+	}
+
+	// getters
+	T getX() const { return x; }
+	T getY() const { return y; }
+	T getZ() const { return z; }
+
+	// setters
+	void setX(T x) { this->x = x; }
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	void setX(U x) { this->x = std::round(x); }
 
-		template<typename U>
-		explicit Vector(const U *array);
 
-		explicit Vector(T inVal);
-		Vector(T inX, T inY, T inZ);
+	void setY(T y) { this->y = y; }
 
-		Vector(const Vector<T, 1> &inX, const T &inY, const T &inZ);
-		Vector(const T &inX, const Vector<T, 1> &inY, const T &inZ);
-		Vector(const T &inX, const T &inY, const Vector<T, 1> &inZ);
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	void setY(U y) { this->x = std::round(y); }
 
-		Vector(const Vector<T, 1> &inX, const T &inY, const Vector<T, 1> &inZ);
-		Vector(const Vector<T, 1> &inX, const Vector<T, 1> &inY, const T &inZ);
-		Vector(const T &inX, const Vector<T, 1> &inY, const Vector<T, 1> &inZ);
 
-		Vector(const Vector<T, 1> &inX, const Vector<T, 1> &inY, const Vector<T, 1> &inZ);
+	void setZ(T z) { this->z = z; }
 
-		Vector(const Vector<T,2> &inXY, const T &inZ);
-		Vector(const T &inX, const Vector<T, 2> &inYZ);
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	void setZ(U z) { this->z = std::round(z); }
 
-		Vector(const Vector<T, 2> &inXY, const Vector<T, 1> &inZ);
-		Vector(const Vector<T, 1> &inX, const Vector<T, 2> &inYZ);
-
-		template<typename U>
-		Vector(const Vector<U, 3> &rhs);
-		Vector(const Matrix<T, 3, 1> &rhs);
-
-
-		inline void
-		set(const T& x, const T& y, const T& z);
-
-
-		inline void
-		setX(const T& value);
-
-		inline void
-		setY(const T& value);
-
-		inline void
-		setZ(const T& value);
-
-
-		inline const T&
-		getX() const;
-
-		inline const T&
-		getY() const;
-
-		inline const T&
-		getZ() const;
-
-
-		Vector& operator = (const Matrix<T, 3, 1> &rhs);
-
-		bool operator == (const Vector &rhs) const;
-		bool operator != (const Vector &rhs) const;
-		bool operator < (const Vector &rhs) const;
-		bool operator <= (const Vector &rhs) const;
-		bool operator > (const Vector &rhs) const;
-		bool operator >= (const Vector &rhs) const;
-
-		const T& operator [] (uint8_t index) const;
-		T& operator [] (uint8_t index);
-		T* ptr();
-		const T* ptr() const;
-
-		Vector operator - () const;
-		Vector operator + (const Vector &rhs) const;
-		Vector operator - (const Vector &rhs) const;
-		T operator * (const Vector &rhs) const;
-		Vector operator ^ (const Vector &rhs) const;
-		Vector operator * (const T &rhs) const;
-		Vector operator / (const T &rhs) const;
-
-		Vector& operator += (const Vector &rhs);
-		Vector& operator -= (const Vector &rhs);
-		Vector& operator *= (const T &rhs);
-		Vector& operator /= (const T &rhs);
-
-		float getLength() const;
-		float getLengthSquared() const;
-
-		Vector scaled(float newLength) const;
-		void scale(float newLength);
-
-		Vector normalized() const;
-		void normalize();
-
-		Matrix<T, 3, 1>&
-		asMatrix();
-
-		const Matrix<T, 3, 1>&
-		asMatrix() const;
-
-		Matrix<T, 1, 3>&
-		asTransposedMatrix();
-
-		const Matrix<T, 1, 3>&
-		asTransposedMatrix() const;
-
-		bool hasNan() const;
-		bool hasInf() const;
-
-	#ifndef __DOXYGEN__
-		IMPLEMENT_VECTOR_ACCESSOR2(x,x); IMPLEMENT_VECTOR_ACCESSOR2(x,y); IMPLEMENT_VECTOR_ACCESSOR2(x,z);
-		IMPLEMENT_VECTOR_ACCESSOR2(y,x); IMPLEMENT_VECTOR_ACCESSOR2(y,y); IMPLEMENT_VECTOR_ACCESSOR2(y,z);
-		IMPLEMENT_VECTOR_ACCESSOR2(z,x); IMPLEMENT_VECTOR_ACCESSOR2(z,y); IMPLEMENT_VECTOR_ACCESSOR2(z,z);
-
-		IMPLEMENT_VECTOR_ACCESSOR3(x,x,x); IMPLEMENT_VECTOR_ACCESSOR3(x,x,y); IMPLEMENT_VECTOR_ACCESSOR3(x,x,z);
-		IMPLEMENT_VECTOR_ACCESSOR3(x,y,x); IMPLEMENT_VECTOR_ACCESSOR3(x,y,y); IMPLEMENT_VECTOR_ACCESSOR3(x,y,z);
-		IMPLEMENT_VECTOR_ACCESSOR3(x,z,x); IMPLEMENT_VECTOR_ACCESSOR3(x,z,y); IMPLEMENT_VECTOR_ACCESSOR3(x,z,z);
-		IMPLEMENT_VECTOR_ACCESSOR3(y,x,x); IMPLEMENT_VECTOR_ACCESSOR3(y,x,y); IMPLEMENT_VECTOR_ACCESSOR3(y,x,z);
-		IMPLEMENT_VECTOR_ACCESSOR3(y,y,x); IMPLEMENT_VECTOR_ACCESSOR3(y,y,y); IMPLEMENT_VECTOR_ACCESSOR3(y,y,z);
-		IMPLEMENT_VECTOR_ACCESSOR3(y,z,x); IMPLEMENT_VECTOR_ACCESSOR3(y,z,y); IMPLEMENT_VECTOR_ACCESSOR3(y,z,z);
-		IMPLEMENT_VECTOR_ACCESSOR3(z,x,x); IMPLEMENT_VECTOR_ACCESSOR3(z,x,y); IMPLEMENT_VECTOR_ACCESSOR3(z,x,z);
-		IMPLEMENT_VECTOR_ACCESSOR3(z,y,x); IMPLEMENT_VECTOR_ACCESSOR3(z,y,y); IMPLEMENT_VECTOR_ACCESSOR3(z,y,z);
-		IMPLEMENT_VECTOR_ACCESSOR3(z,z,x); IMPLEMENT_VECTOR_ACCESSOR3(z,z,y); IMPLEMENT_VECTOR_ACCESSOR3(z,z,z);
-
-		IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,x); IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,y); IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,x); IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,y); IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(x,x,z,x); IMPLEMENT_VECTOR_ACCESSOR4(x,x,z,y); IMPLEMENT_VECTOR_ACCESSOR4(x,x,z,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,x); IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,y); IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,x); IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,y); IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(x,y,z,x); IMPLEMENT_VECTOR_ACCESSOR4(x,y,z,y); IMPLEMENT_VECTOR_ACCESSOR4(x,y,z,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(x,z,x,x); IMPLEMENT_VECTOR_ACCESSOR4(x,z,x,y); IMPLEMENT_VECTOR_ACCESSOR4(x,z,x,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(x,z,y,x); IMPLEMENT_VECTOR_ACCESSOR4(x,z,y,y); IMPLEMENT_VECTOR_ACCESSOR4(x,z,y,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(x,z,z,x); IMPLEMENT_VECTOR_ACCESSOR4(x,z,z,y); IMPLEMENT_VECTOR_ACCESSOR4(x,z,z,z);
-
-		IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,x); IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,y); IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,x); IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,y); IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(y,x,z,x); IMPLEMENT_VECTOR_ACCESSOR4(y,x,z,y); IMPLEMENT_VECTOR_ACCESSOR4(y,x,z,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,x); IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,y); IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,x); IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,y); IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(y,y,z,x); IMPLEMENT_VECTOR_ACCESSOR4(y,y,z,y); IMPLEMENT_VECTOR_ACCESSOR4(y,y,z,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(y,z,x,x); IMPLEMENT_VECTOR_ACCESSOR4(y,z,x,y); IMPLEMENT_VECTOR_ACCESSOR4(y,z,x,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(y,z,y,x); IMPLEMENT_VECTOR_ACCESSOR4(y,z,y,y); IMPLEMENT_VECTOR_ACCESSOR4(y,z,y,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(y,z,z,x); IMPLEMENT_VECTOR_ACCESSOR4(y,z,z,y); IMPLEMENT_VECTOR_ACCESSOR4(y,z,z,z);
-
-		IMPLEMENT_VECTOR_ACCESSOR4(z,x,x,x); IMPLEMENT_VECTOR_ACCESSOR4(z,x,x,y); IMPLEMENT_VECTOR_ACCESSOR4(z,x,x,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(z,x,y,x); IMPLEMENT_VECTOR_ACCESSOR4(z,x,y,y); IMPLEMENT_VECTOR_ACCESSOR4(z,x,y,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(z,x,z,x); IMPLEMENT_VECTOR_ACCESSOR4(z,x,z,y); IMPLEMENT_VECTOR_ACCESSOR4(z,x,z,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(z,y,x,x); IMPLEMENT_VECTOR_ACCESSOR4(z,y,x,y); IMPLEMENT_VECTOR_ACCESSOR4(z,y,x,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(z,y,y,x); IMPLEMENT_VECTOR_ACCESSOR4(z,y,y,y); IMPLEMENT_VECTOR_ACCESSOR4(z,y,y,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(z,y,z,x); IMPLEMENT_VECTOR_ACCESSOR4(z,y,z,y); IMPLEMENT_VECTOR_ACCESSOR4(z,y,z,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(z,z,x,x); IMPLEMENT_VECTOR_ACCESSOR4(z,z,x,y); IMPLEMENT_VECTOR_ACCESSOR4(z,z,x,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(z,z,y,x); IMPLEMENT_VECTOR_ACCESSOR4(z,z,y,y); IMPLEMENT_VECTOR_ACCESSOR4(z,z,y,z);
-		IMPLEMENT_VECTOR_ACCESSOR4(z,z,z,x); IMPLEMENT_VECTOR_ACCESSOR4(z,z,z,y); IMPLEMENT_VECTOR_ACCESSOR4(z,z,z,z);
-	#endif
-
-	public:
-		T x;
-		T y;
-		T z;
-	};
-
-	template<typename U, typename T>
-	static inline Vector<T, 3> operator * (const U &lhs, const Vector<T, 3> &rhs)
-	{
-		return rhs * lhs;
+
+	void set(T x, T y, T z) {
+		this->x = x;
+		this->y = y;
+		this->z = z;
 	}
 
-	template<typename T, typename U>
-	static inline Vector<U, 3> operator * (const Matrix<T, 3, 3> &lhs, const Vector<U, 3> &rhs)
-	{
-		return lhs * rhs.asMatrix();
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	void set(U x, U y, U z) {
+		this->x = std::round(x);
+		this->y = std::round(y);
+		this->z = std::round(z);
 	}
 
+	// accessors
+	T& operator [] (std::size_t index)
+	{ return reinterpret_cast<T*>(this)[index]; }
+
+	const T& operator [] (std::size_t index) const
+	{ return reinterpret_cast<const T*>(this)[index]; }
+
+	T* ptr() { return reinterpret_cast<T*>(this); }
+
+	const T* ptr() const { return reinterpret_cast<const T*>(this); }
+
+	// operators
+	auto operator<=>(const Vector&) const = default;
+
+	constexpr Vector operator+ (const Vector &rhs) const
+	{ return Vector(x + rhs.x, y + rhs.y, z + rhs.z); }
+
+	constexpr Vector operator- (const Vector &rhs) const
+	{ return Vector(x - rhs.x, y - rhs.y, z - rhs.z); }
+
+	constexpr T operator* (const Vector &rhs) const
+	{ return x * rhs.x + y * rhs.y + z * rhs.z; }
+
+	constexpr Vector operator^ (const Vector &rhs) const
+	{ return Vector(y * rhs.z-z * rhs.y, z * rhs.x-x * rhs.z, x * rhs.y-y * rhs.x); }
+
+	template <typename U>
+	constexpr Vector operator* (U scale) const
+	{ return Vector<U, 3>(x * scale, y * scale, z * scale); }
+
+	template <typename U>
+	constexpr Vector operator/ (U scale) const
+	{ return Vector<U, 3>(x / scale, y / scale, z / scale); }
+
+	Vector& operator += (const Vector &rhs)
+	{ x += rhs.x; y += rhs.y; z += rhs.z; return *this; }
+
+	Vector& operator -= (const Vector &rhs)
+	{ x -= rhs.x; y -= rhs.y; z -= rhs.z; return *this; }
+
+	template <typename U>
+	Vector& operator *= (U scale)
+	{ x *= scale; y *= scale; z *= scale; return *this; }
+
+	template <typename U>
+	Vector& operator /= (U scale)
+	{ x /= scale; y /= scale; z /= scale; return *this; }
+
+	// template <typename>
+	// requires std::is_signed<T>::value
+	constexpr Vector operator- () const
+	{ return Vector(-x, -y, -z); }
+
+	// additional methods
+	T getLength() const
+	{ return std::sqrt(getLengthSquared()); }
+
+	T getLength() const
+	requires std::integral<T>
+	{ return round(std::sqrt(getLengthSquared())); }
+
+	WideWideType getLengthSquared() const
+	{ return std::pow(x, 2) + std::pow(y, 2) + std::pow(z, 2); }
+
+	Vector scaled(float newLength) const {
+		float scale = newLength / getLength();
+		return *this * scale;
+	}
+
+	void scale(float newLength)
+	{ *this = scaled(newLength); }
+
+	Vector normalized() const { return scaled(1.0f); }
+	void normalize() { scale(1.0); }
+
+	bool hasNan() const { return std::isnan(x) || std::isnan(y) || std::isnan(z); }
+	bool hasInf() const { return std::isinf(x) || std::isinf(y) || std::isinf(z); }
+
+#ifndef __DOXYGEN__
+	IMPLEMENT_VECTOR_ACCESSOR2(x,x); IMPLEMENT_VECTOR_ACCESSOR2(x,y); IMPLEMENT_VECTOR_ACCESSOR2(x,z);
+	IMPLEMENT_VECTOR_ACCESSOR2(y,x); IMPLEMENT_VECTOR_ACCESSOR2(y,y); IMPLEMENT_VECTOR_ACCESSOR2(y,z);
+	IMPLEMENT_VECTOR_ACCESSOR2(z,x); IMPLEMENT_VECTOR_ACCESSOR2(z,y); IMPLEMENT_VECTOR_ACCESSOR2(z,z);
+
+	IMPLEMENT_VECTOR_ACCESSOR3(x,x,x); IMPLEMENT_VECTOR_ACCESSOR3(x,x,y); IMPLEMENT_VECTOR_ACCESSOR3(x,x,z);
+	IMPLEMENT_VECTOR_ACCESSOR3(x,y,x); IMPLEMENT_VECTOR_ACCESSOR3(x,y,y); IMPLEMENT_VECTOR_ACCESSOR3(x,y,z);
+	IMPLEMENT_VECTOR_ACCESSOR3(x,z,x); IMPLEMENT_VECTOR_ACCESSOR3(x,z,y); IMPLEMENT_VECTOR_ACCESSOR3(x,z,z);
+	IMPLEMENT_VECTOR_ACCESSOR3(y,x,x); IMPLEMENT_VECTOR_ACCESSOR3(y,x,y); IMPLEMENT_VECTOR_ACCESSOR3(y,x,z);
+	IMPLEMENT_VECTOR_ACCESSOR3(y,y,x); IMPLEMENT_VECTOR_ACCESSOR3(y,y,y); IMPLEMENT_VECTOR_ACCESSOR3(y,y,z);
+	IMPLEMENT_VECTOR_ACCESSOR3(y,z,x); IMPLEMENT_VECTOR_ACCESSOR3(y,z,y); IMPLEMENT_VECTOR_ACCESSOR3(y,z,z);
+	IMPLEMENT_VECTOR_ACCESSOR3(z,x,x); IMPLEMENT_VECTOR_ACCESSOR3(z,x,y); IMPLEMENT_VECTOR_ACCESSOR3(z,x,z);
+	IMPLEMENT_VECTOR_ACCESSOR3(z,y,x); IMPLEMENT_VECTOR_ACCESSOR3(z,y,y); IMPLEMENT_VECTOR_ACCESSOR3(z,y,z);
+	IMPLEMENT_VECTOR_ACCESSOR3(z,z,x); IMPLEMENT_VECTOR_ACCESSOR3(z,z,y); IMPLEMENT_VECTOR_ACCESSOR3(z,z,z);
+
+	IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,x); IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,y); IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,x); IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,y); IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(x,x,z,x); IMPLEMENT_VECTOR_ACCESSOR4(x,x,z,y); IMPLEMENT_VECTOR_ACCESSOR4(x,x,z,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,x); IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,y); IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,x); IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,y); IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(x,y,z,x); IMPLEMENT_VECTOR_ACCESSOR4(x,y,z,y); IMPLEMENT_VECTOR_ACCESSOR4(x,y,z,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(x,z,x,x); IMPLEMENT_VECTOR_ACCESSOR4(x,z,x,y); IMPLEMENT_VECTOR_ACCESSOR4(x,z,x,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(x,z,y,x); IMPLEMENT_VECTOR_ACCESSOR4(x,z,y,y); IMPLEMENT_VECTOR_ACCESSOR4(x,z,y,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(x,z,z,x); IMPLEMENT_VECTOR_ACCESSOR4(x,z,z,y); IMPLEMENT_VECTOR_ACCESSOR4(x,z,z,z);
+
+	IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,x); IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,y); IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,x); IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,y); IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(y,x,z,x); IMPLEMENT_VECTOR_ACCESSOR4(y,x,z,y); IMPLEMENT_VECTOR_ACCESSOR4(y,x,z,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,x); IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,y); IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,x); IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,y); IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(y,y,z,x); IMPLEMENT_VECTOR_ACCESSOR4(y,y,z,y); IMPLEMENT_VECTOR_ACCESSOR4(y,y,z,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(y,z,x,x); IMPLEMENT_VECTOR_ACCESSOR4(y,z,x,y); IMPLEMENT_VECTOR_ACCESSOR4(y,z,x,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(y,z,y,x); IMPLEMENT_VECTOR_ACCESSOR4(y,z,y,y); IMPLEMENT_VECTOR_ACCESSOR4(y,z,y,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(y,z,z,x); IMPLEMENT_VECTOR_ACCESSOR4(y,z,z,y); IMPLEMENT_VECTOR_ACCESSOR4(y,z,z,z);
+
+	IMPLEMENT_VECTOR_ACCESSOR4(z,x,x,x); IMPLEMENT_VECTOR_ACCESSOR4(z,x,x,y); IMPLEMENT_VECTOR_ACCESSOR4(z,x,x,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(z,x,y,x); IMPLEMENT_VECTOR_ACCESSOR4(z,x,y,y); IMPLEMENT_VECTOR_ACCESSOR4(z,x,y,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(z,x,z,x); IMPLEMENT_VECTOR_ACCESSOR4(z,x,z,y); IMPLEMENT_VECTOR_ACCESSOR4(z,x,z,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(z,y,x,x); IMPLEMENT_VECTOR_ACCESSOR4(z,y,x,y); IMPLEMENT_VECTOR_ACCESSOR4(z,y,x,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(z,y,y,x); IMPLEMENT_VECTOR_ACCESSOR4(z,y,y,y); IMPLEMENT_VECTOR_ACCESSOR4(z,y,y,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(z,y,z,x); IMPLEMENT_VECTOR_ACCESSOR4(z,y,z,y); IMPLEMENT_VECTOR_ACCESSOR4(z,y,z,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(z,z,x,x); IMPLEMENT_VECTOR_ACCESSOR4(z,z,x,y); IMPLEMENT_VECTOR_ACCESSOR4(z,z,x,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(z,z,y,x); IMPLEMENT_VECTOR_ACCESSOR4(z,z,y,y); IMPLEMENT_VECTOR_ACCESSOR4(z,z,y,z);
+	IMPLEMENT_VECTOR_ACCESSOR4(z,z,z,x); IMPLEMENT_VECTOR_ACCESSOR4(z,z,z,y); IMPLEMENT_VECTOR_ACCESSOR4(z,z,z,z);
+#endif
+
+	// depricated methods
+	template<typename U>
+	[[deprecated("Use common constructor instead!")]]
+	Vector<U, 3>
+	convert() const { return {*this}; }
+
+protected:
+	template<typename U>
+	friend IOStream&
+	operator<<(IOStream& os, const Vector<U, 3>& c);
+};
+
+template<typename U, typename T>
+static inline Vector<T, 3> operator * (const U &lhs, const Vector<T, 3> &rhs)
+{
+	return rhs * lhs;
+}
+
+template<typename T, typename U>
+static inline Vector<U, 3> operator * (const Matrix<T, 3, 3> &lhs, const Vector<U, 3> &rhs)
+{
+	return lhs * rhs.asMatrix();
+}
 
-	typedef Vector<float, 3> 	Vector3f;
-	typedef Vector<int16_t, 3>	Vector3i;
-	typedef Vector<uint16_t, 3>	Vector3u;
+// TODO Whats with these?
+// template<typename T>
+// static inline Vector<T,3>
+// operator * (const T &lhs, const Vector<T,3> &rhs)
+// {
+// 	return rhs * lhs;
+// }
+
+// template<typename T, typename U>
+// static inline Vector<T,3>
+// operator * (const Matrix<T, 3, 3> &lhs, const Vector<U,3> &rhs)
+// {
+// 	return lhs * rhs.asTMatrix();
+// }
+
+#if __has_include(<modm/io/iostream.hpp>)
+#include <modm/io/iostream.hpp>
+
+template<typename U>
+IOStream&
+operator<< (IOStream& os, const Vector<U, 3>& v) {
+	os << "x=" << v.x << "\ty=" << v.y << "\tz=" << v.z;
+	return os;
 }
+#endif
 
-#include "vector3_impl.hpp"
+using Vector3f = Vector<float, 3>;
+using Vector3i = Vector<int16_t, 3>;
+using Vector3u = Vector<uint16_t, 3>;
 
-#endif // MODM_VECTOR3_HPP
+} // namespace modm
\ No newline at end of file
diff --git a/src/modm/math/geometry/vector3_impl.hpp b/src/modm/math/geometry/vector3_impl.hpp
deleted file mode 100644
index 70b37af96d..0000000000
--- a/src/modm/math/geometry/vector3_impl.hpp
+++ /dev/null
@@ -1,538 +0,0 @@
-/*
- * Copyright (c) 2011-2012, Fabian Greif
- * Copyright (c) 2012, Niklas Hauser
- *
- * This file is part of the modm project.
- *
- * This Source Code Form is subject to the terms of the Mozilla Public
- * License, v. 2.0. If a copy of the MPL was not distributed with this
- * file, You can obtain one at http://mozilla.org/MPL/2.0/.
- */
-// ----------------------------------------------------------------------------
-
-#ifndef MODM_VECTOR3_HPP
-	#error	"Don't include this file directly, use 'vector3.hpp' instead!"
-#endif
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>::Vector() :
-	x(),
-	y(),
-	z()
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-template<typename U>
-modm::Vector<T, 3>::Vector(const U *array) :
-	x(array[0]),
-	y(array[1]),
-	z(array[2])
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>::Vector(T inX, T inY, T inZ) :
-	x(inX),
-	y(inY),
-	z(inZ)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>::Vector(const modm::Vector<T, 1> &inX,
-		const modm::Vector<T, 1> &inY,
-		const modm::Vector<T, 1> &inZ) :
-	x(inX.x),
-	y(inY.x),
-	z(inZ.x)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>::Vector(const T &inX,
-		const modm::Vector<T, 1> &inY,
-		const modm::Vector<T, 1> &inZ) :
-	x(inX),
-	y(inY.x),
-	z(inZ.x)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>::Vector(const modm::Vector<T, 1> &inX, const T &inY, const modm::Vector<T, 1> &inZ) :
-	x(inX.x),
-	y(inY),
-	z(inZ.x)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>::Vector(const modm::Vector<T, 1> &inX, const modm::Vector<T, 1> &inY, const T &inZ)
-:
-	x(inX.x),
-	y(inY.x),
-	z(inZ)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>::Vector(const modm::Vector<T, 1> &inX, const T &inY, const T &inZ) :
-	x(inX.x),
-	y(inY),
-	z(inZ)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>::Vector(const T &inX, const modm::Vector<T, 1> &inY, const T &inZ) :
-	x(inX),
-	y(inY.x),
-	z(inZ)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>::Vector(const T &inX, const T &inY, const modm::Vector<T, 1> &inZ) :
-	x(inX),
-	y(inY),
-	z(inZ.x)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>::Vector(const modm::Vector<T, 2> &inXY, const T &inZ) :
-	x(inXY.x),
-	y(inXY.y),
-	z(inZ)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>::Vector(const modm::Vector<T, 2> &inXY, const modm::Vector<T, 1> &inZ) :
-	x(inXY.x),
-	y(inXY.y),
-	z(inZ.x)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>::Vector(const T &inX, const modm::Vector<T, 2> &inYZ) :
-	x(inX),
-	y(inYZ.x),
-	z(inYZ.y)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>::Vector(const modm::Vector<T, 1> &inX, const modm::Vector<T, 2> &inYZ) :
-	x(inX.x),
-	y(inYZ.x),
-	z(inYZ.y)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>::Vector(T inVal) :
-	x(inVal),
-	y(inVal),
-	z(inVal)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>::Vector(const modm::Matrix<T, 3, 1> &rhs) :
-	x(reinterpret_cast<const T*>(&rhs)[0]),
-	y(reinterpret_cast<const T*>(&rhs)[1]),
-	z(reinterpret_cast<const T*>(&rhs)[2])
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-template<typename U>
-modm::Vector<T, 3>::Vector(const modm::Vector<U, 3> &rhs) :
-	x(rhs.x),
-	y(rhs.y),
-	z(rhs.z)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-void
-modm::Vector<T, 3>::set(const T& x, const T& y, const T& z)
-{
-	this->x = x;
-	this->y = y;
-	this->z = z;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-void
-modm::Vector<T, 3>::setX(const T& value)
-{
-	this->x = value;
-}
-
-template<typename T>
-void
-modm::Vector<T, 3>::setY(const T& value)
-{
-	this->y = value;
-}
-
-template<typename T>
-void
-modm::Vector<T, 3>::setZ(const T& value)
-{
-	this->z = value;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-const T&
-modm::Vector<T, 3>::getX() const
-{
-	return this->x;
-}
-
-template<typename T>
-const T&
-modm::Vector<T, 3>::getY() const
-{
-	return this->y;
-}
-
-template<typename T>
-const T&
-modm::Vector<T, 3>::getZ() const
-{
-	return this->z;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>&
-modm::Vector<T, 3>::operator = (const modm::Matrix<T, 3, 1> &rhs)
-{
-	x = reinterpret_cast<const T*>(&rhs)[0];
-	y = reinterpret_cast<const T*>(&rhs)[1];
-	z = reinterpret_cast<const T*>(&rhs)[2];
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 3>::operator == (const Vector &rhs) const
-{
-	return (rhs.x == x) && (rhs.y == y) && (rhs.z == z);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 3>::operator != (const Vector &rhs) const
-{
-	return (rhs.x != x) || (rhs.y != y) || (rhs.z != z);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 3>::operator < (const Vector &rhs) const
-{
-	return (x < rhs.x) || ((x == rhs.x) && ((y < rhs.y) || ((y == rhs.y) && (z < rhs.z))));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 3>::operator <= (const Vector &rhs) const
-{
-	return (x < rhs.x) || ((x == rhs.x) && ((y < rhs.y) || ((y == rhs.y) && (z <= rhs.z))));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 3>::operator > (const Vector &rhs) const
-{
-	return (x > rhs.x) || ((x == rhs.x) && ((y > rhs.y) || ((y == rhs.y) && (z > rhs.z))));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 3>::operator >= (const Vector &rhs) const
-{
-	return (x > rhs.x) || ((x == rhs.x) && ((y > rhs.y) || ((y == rhs.y) && (z >= rhs.z))));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-const T&
-modm::Vector<T, 3>::operator [] (uint8_t index) const
-{
-	return reinterpret_cast<const T*>(this)[index];
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-T&
-modm::Vector<T, 3>::operator [] (uint8_t index)
-{
-	return reinterpret_cast<T*>(this)[index];
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-T*
-modm::Vector<T, 3>::ptr()
-{
-	return reinterpret_cast<T*>(this);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-const T*
-modm::Vector<T, 3>::ptr() const
-{
-	return reinterpret_cast<const T*>(this);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>
-modm::Vector<T, 3>::operator + (const modm::Vector<T, 3> &rhs) const
-{
-	return modm::Vector<T, 3>(x+rhs.x, y+rhs.y, z+rhs.z);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>
-modm::Vector<T, 3>::operator - (const modm::Vector<T, 3> &rhs) const
-{
-	return modm::Vector<T, 3>(x-rhs.x, y-rhs.y, z-rhs.z);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-T
-modm::Vector<T, 3>::operator * (const modm::Vector<T, 3> &rhs) const
-{
-	return x*rhs.x + y*rhs.y + z*rhs.z;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>
-modm::Vector<T, 3>::operator ^ (const modm::Vector<T, 3> &rhs) const
-{
-	return modm::Vector<T, 3>(y*rhs.z-z*rhs.y, z*rhs.x-x*rhs.z, x*rhs.y-y*rhs.x);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>
-modm::Vector<T, 3>::operator * (const T &rhs) const
-{
-	return modm::Vector<T, 3>(x*rhs, y*rhs, z*rhs);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>
-modm::Vector<T, 3>::operator / (const T &rhs) const
-{
-	return modm::Vector<T, 3>(x/rhs, y/rhs, z/rhs);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>&
-modm::Vector<T, 3>::operator += (const modm::Vector<T, 3> &rhs)
-{
-	x += rhs.x;
-	y += rhs.y;
-	z += rhs.z;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>&
-modm::Vector<T, 3>::operator -= (const modm::Vector<T, 3> &rhs)
-{
-	x -= rhs.x;
-	y -= rhs.y;
-	z -= rhs.z;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>&
-modm::Vector<T, 3>::operator *= (const T &rhs)
-{
-	x *= rhs;
-	y *= rhs;
-	z *= rhs;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>&
-modm::Vector<T, 3>::operator /= (const T &rhs)
-{
-	x /= rhs;
-	y /= rhs;
-	z /= rhs;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>
-modm::Vector<T, 3>::operator - () const
-{
-	return modm::Vector<T, 3>(-x, -y, -z);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-float
-modm::Vector<T, 3>::getLength() const
-{
-	return std::sqrt(getLengthSquared());
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-float
-modm::Vector<T, 3>::getLengthSquared() const
-{
-	return x*x+y*y+z*z;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-void
-modm::Vector<T, 3>::scale(float newLength)
-{
-	*this = scaled(newLength);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>
-modm::Vector<T, 3>::scaled(float newLength) const
-{
-	float scale = newLength/getLength();
-	return *this * scale;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-void
-modm::Vector<T, 3>::normalize()
-{
-	scale(1.0);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 3>
-modm::Vector<T, 3>::normalized() const
-{
-	return scaled(1.0f);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Matrix<T, 3, 1>&
-modm::Vector<T, 3>::asMatrix()
-{
-	return *(modm::Matrix<T, 3, 1>*)this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-const modm::Matrix<T, 3, 1>&
-modm::Vector<T, 3>::asMatrix() const
-{
-	return *(modm::Matrix<T, 3, 1>*)this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Matrix<T, 1, 3>&
-modm::Vector<T, 3>::asTransposedMatrix()
-{
-	return *(modm::Matrix<T, 1, 3>*)this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-const modm::Matrix<T, 1, 3>&
-modm::Vector<T, 3>::asTransposedMatrix() const
-{
-	return *(modm::Matrix<T, 1, 3>*)this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 3>::hasNan() const
-{
-	return std::isnan(x) || std::isnan(y) || std::isnan(z);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 3>::hasInf() const
-{
-	return std::isinf(x) || std::isinf(y) || std::isinf(z);
-}
-
-// ----------------------------------------------------------------------------
-//template<typename T>
-//static inline modm::Vector<T,3> operator * (const T &lhs, const modm::Vector<T,3> &rhs)
-//{
-//	return rhs * lhs;
-//}
-//
-//
-//// ----------------------------------------------------------------------------
-//template<typename T>
-//static inline modm::Vector<T,3> operator * (const modm::Matrix<T, 3, 3> &lhs, const modm::Vector<T,3> &rhs)
-//{
-//	return lhs * rhs.asTMatrix();
-//}
diff --git a/src/modm/math/geometry/vector4.hpp b/src/modm/math/geometry/vector4.hpp
index c91bcbed9d..f9b0416fb7 100644
--- a/src/modm/math/geometry/vector4.hpp
+++ b/src/modm/math/geometry/vector4.hpp
@@ -2,6 +2,7 @@
  * Copyright (c) 2011-2012, Fabian Greif
  * Copyright (c) 2012, Georgi Grinshpun
  * Copyright (c) 2012, Niklas Hauser
+ * Copyright (c) 2022, Thomas Sommer
  *
  * This file is part of the modm project.
  *
@@ -11,252 +12,377 @@
  */
 // ----------------------------------------------------------------------------
 
-#ifndef MODM_VECTOR4_HPP
-#define MODM_VECTOR4_HPP
+#pragma once
 
-#include <stdint.h>
+#include <concepts>
 #include "vector.hpp"
 
 namespace modm
 {
-	/**
-	 * \brief	Class for handling common vector operations (4D)
-	 *
-	 *	+ : addition of points
-	 *	- : different of points
-	 *	* : dot product or scalar multiplication
-	 *	/ : scalar division
-	 *
-	 * Adapted from the implementation of Gaspard Petit (gaspardpetit@gmail.com).
-	 *
-	 * \see <a href"http://www-etud.iro.umontreal.ca/~petitg/cpp/point.html">Homepage</a>
-	 *
-	 * \ingroup	modm_math_geometry
-	 * \author	Niklas Hauser
-	 */
-	template<typename T>
-	class Vector<T, 4>
-	{
-	public:
-		Vector();
-
-		explicit Vector(T inVal);
-		Vector(T inX, T inY, T inZ, T inW);
-
-		Vector(const Vector<T, 1> &inX, const Vector<T, 1> &inY, const Vector<T, 1> &inZ, const Vector<T, 1> &inW);
-		Vector(const Vector<T, 1> &inX, const Vector<T, 1> &inY, const Vector<T, 1> &inZ, const T &inW);
-		Vector(const Vector<T, 1> &inX, const Vector<T, 1> &inY, const T &inZ, const T &inW);
-		Vector(const Vector<T, 1> &inX, const T &inY, const Vector<T, 1> &inZ, const T &inW);
-		Vector(const T &inX, const Vector<T, 1> &inY, const Vector<T, 1> &inZ, const T &inW);
-		Vector(const Vector<T, 1> &inX, const T &inY, const T &inZ, const T &inW);
-		Vector(const T &inX, const Vector<T, 1> &inY, const T &inZ, const T &inW);
-		Vector(const Vector<T, 1> &inX, const Vector<T, 1> &inY, const T &inZ, const Vector<T, 1> &inW);
-		Vector(const Vector<T, 1> &inX, const T &inY, const T &inZ, const Vector<T, 1> &inW);
-		Vector(const T &inX, const Vector<T, 1> &inY, const T &inZ, const Vector<T, 1> &inW);
-		Vector(const T &inX, const T &inY, const T &inZ, const Vector<T, 1> &inW);
-		Vector(const Vector<T, 1> &inX, const T &inY, const Vector<T, 1> &inZ, const Vector<T, 1> &inW);
-		Vector(const T &inX, const T &inY, const Vector<T, 1> &inZ, const Vector<T, 1> &inW);
-		Vector(const T &inX, const Vector<T, 1> &inY, const Vector<T, 1> &inZ, const Vector<T, 1> &inW);
+/**
+ * \brief	Class for handling common vector operations (4D)
+ *
+ *	+ : addition of points
+	*	- : different of points
+	*	* : dot product or scalar multiplication
+	*	/ : scalar division
+	*
+	* Adapted from the implementation of Gaspard Petit (gaspardpetit@gmail.com).
+	*
+	* \see <a href"http://www-etud.iro.umontreal.ca/~petitg/cpp/point.html">Homepage</a>
+	*
+	* \author	Niklas Hauser
+	* \author	Thomas Sommer
+	* \ingroup	modm_math_geometry
+	*/
+template<typename T>
+class Vector<T, 4>
+{
+	using VecT1 = Vector<T, 1>;
+	using VecT2 = Vector<T, 2>;
+	using VecT3 = Vector<T, 3>;
+	using VecT4 = Vector;
+
+public:
+	using WideType = GeometricTraits<T>::WideType;
+	using WideWideType = GeometricTraits<WideType>::WideType;
+	using FloatType = GeometricTraits<T>::FloatType;
+
+	T x{0}, y{0}, z{0}, w{0};
+
+	// basic constructors
+	constexpr Vector() = default;
+
+	constexpr explicit Vector(T xyzw)
+		: x(xyzw), y(xyzw), z(xyzw), w(xyzw) {}
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	constexpr explicit Vector(T xyzw)
+		: x(std::round(xyzw)), y(std::round(xyzw)), z(std::round(xyzw)), w(std::round(xyzw)) {}
+
+	constexpr Vector(T x, T y, T z, T w)
+		: x(x), y(y), z(z), w(w) {}
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	constexpr Vector(U x, U y, U z, U w)
+		: x(std::round(x)), y(std::round(y)), z(std::round(z)), w(std::round(w)) {}
+
+	template<typename U>
+	constexpr Vector(const Vector<U, 4> &v) : Vector(v.x, v.y, v.z, v.w) {}
+
+	// vector constructors
+	constexpr Vector(VecT1 vx, VecT1 vy, VecT1 vz, VecT1 vw) : Vector(vx.x, vy.x, vz.x, vw.x) {}
+	constexpr Vector(VecT1 vx, VecT1 vy, VecT1 vz, T w) : Vector(vx.x, vy.x, vz.x, w) {}
+	constexpr Vector(VecT1 vx, VecT1 vy, T z, T w) : Vector(vx.x, vy.x, z, w) {}
+	constexpr Vector(VecT1 vx, T y, VecT1 vz, T w) : Vector(vx.x, y, vz.x, w) {}
+	constexpr Vector(T x, VecT1 vy, VecT1 vz, T w) : Vector(x, vy.x, vz.x, w) {}
+	constexpr Vector(VecT1 vx, T y, T z, T w) : Vector(vx.x, y, z, w) {}
+	constexpr Vector(T x, VecT1 vy, T z, T w) : Vector(x, vy.x, z, w) {}
+	constexpr Vector(VecT1 vx, VecT1 vy, T z, VecT1 vw) : Vector(vx.x, vy.x, z, vw.x) {}
+	constexpr Vector(VecT1 vx, T y, T z, VecT1 vw) : Vector(vx.x, y, z, vw.x) {}
+	constexpr Vector(T x, VecT1 vy, T z, VecT1 vw) : Vector(x, vy.x, z, vw.x) {}
+	constexpr Vector(T x, T y, T z, VecT1 vw) : Vector(x, y, z, vw.x) {}
+	constexpr Vector(VecT1 vx, T y, VecT1 vy, VecT1 vz, VecT1 vw) : Vector(vx.x, y, vy.x, vz.x, vw.x) {}
+	constexpr Vector(T x, T y, VecT1 vz, VecT1 vw) : Vector(x, y, vz.x, vw.x) {}
+	constexpr Vector(T x, VecT1 vy, VecT1 vz, VecT1 vw) : Vector(x, vy.x, vz.x, vw.x) {}
+
+	constexpr Vector(VecT2 vxy, VecT1 vz, VecT1 vw) : Vector(vxy.x, vxy.y, vz.x, vw.x) {}
+	constexpr Vector(VecT2 vxy, VecT1 vz, T w) : Vector(vxy.x, vxy.y, vz.x, w) {}
+	constexpr Vector(VecT2 vxy, T z, T w) : Vector(vxy.x, vxy.y, z, w) {}
+	constexpr Vector(VecT2 vxy, T z, VecT1 w) : Vector(vxy.x, vxy.y, z, w.x) {}
+
+	constexpr Vector(VecT1 vx, VecT2 vyz, VecT1 vw) : Vector(vx.x, vyz.x, vyz.y, vw.x) {}
+	constexpr Vector(VecT1 vx, VecT2 vyz, T w) : Vector(vx.x, vyz.x, vyz.y, w) {}
+	constexpr Vector(T x, VecT2 vyz, T w) : Vector(x, vyz.x, vyz.y, w) {}
+	constexpr Vector(T x, VecT2 vyz, VecT1 vw) : Vector(x, vyz.x, vyz.y, vw.x) {}
+
+	constexpr Vector(VecT1 vx, VecT1 vy, VecT2 vzw) : Vector(vx.x, vy.x, vzw.x, vzw.y) {}
+	constexpr Vector(VecT1 vx, T y, VecT2 vzw) : Vector(vx.x, y, vzw.x, vzw.y) {}
+	constexpr Vector(T x, T y, VecT2 vzw) : Vector(x, y, vzw.x, vzw.y) {}
+	constexpr Vector(T x, VecT1 vy, VecT2 vzw) : Vector(x, vy.x, vzw.x, vzw.y) {}
+
+	constexpr Vector(VecT2 vxy, VecT2 vzw) : Vector(vxy.x, vxy.y, vzw.x, vzw.y) {}
+
+	constexpr Vector(const VecT3 &vxyz, T w) : Vector(vxyz.x, vxyz.y, vxyz.z, w) {}
+	constexpr Vector(const VecT3 &vxyz, VecT1 vw) : Vector(vxyz.x, vxyz.y, vxyz.z, vw.x) {}
+
+	constexpr Vector(VecT1 vx, const VecT3 &vyzw) : Vector(vx.x, vyzw.x, vyzw.y, vyzw.z) {}
+	constexpr Vector(T x, const VecT3 &vyzw) : Vector(x, vyzw.x, vyzw.y, vyzw.z) {}
+
+	// matrix constructors
+	constexpr Vector(const Matrix<T, 4, 1> &rhs)
+		: x(reinterpret_cast<const T*>(&rhs)[0]),
+		y(reinterpret_cast<const T*>(&rhs)[1]),
+		z(reinterpret_cast<const T*>(&rhs)[2]),
+		w(reinterpret_cast<const T*>(&rhs)[3])
+	{}
+
+	// array constructors
+	template<typename U>
+	constexpr explicit Vector(U *array) : Vector(array[0], array[1], array[2], array[3]) {}
+
+	// as matrix convertors
+	Matrix<T, 4, 1>&
+	asMatrix() { return *(Matrix<T, 4, 1>*)this; }
+
+	const Matrix<T, 4, 1>&
+	asMatrix() const { return *(Matrix<T, 4, 1>*)this; }
+
+	Matrix<T, 1, 4>&
+	asTransposedMatrix() { return *(Matrix<T, 1, 4>*)this; }
+
+	const Matrix<T, 1, 4>&
+	asTransposedMatrix() const { return *(Matrix<T, 1, 4>*)this; }
+
+	// matrix assignment
+	Vector& operator= (const Matrix<T, 4, 1> &rhs) {
+		x = reinterpret_cast<const T*>(&rhs)[0];
+		y = reinterpret_cast<const T*>(&rhs)[1];
+		z = reinterpret_cast<const T*>(&rhs)[2];
+		w = reinterpret_cast<const T*>(&rhs)[3];
+
+		return *this;
+	}
+
+	// getters
+	T getX() const { return x; }
+	T getY() const { return y; }
+	T getZ() const { return z; }
+	T getW() const { return w; }
+
+	// setters
+	void setX(T x) { this->x = x; }
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	void setX(U x) { this->x = std::round(x); }
+
+
+	void setY(T y) { this->y = y; }
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	void setY(U y) { this->x = std::round(y); }
+
 
-		Vector(const Vector<T, 2> &inXY, const Vector<T, 1> &inZ, const Vector<T, 1> &inW);
-		Vector(const Vector<T, 2> &inXY, const Vector<T, 1> &inZ, const T &inW);
-		Vector(const Vector<T, 2> &inXY, const T &inZ, const T &inW);
-		Vector(const Vector<T, 2> &inXY, const T &inZ, const Vector<T, 1> &inW);
-
-		Vector(const Vector<T, 1> &inX, const Vector<T, 2> &inYZ, const Vector<T, 1> &inW);
-		Vector(const Vector<T, 1> &inX, const Vector<T, 2> &inYZ, const T &inW);
-		Vector(const T &inX, const Vector<T, 2> &inYZ, const T &inW);
-		Vector(const T &inX, const Vector<T, 2> &inYZ, const Vector<T, 1> &inW);
-
-		Vector(const Vector<T, 1> &inX, const Vector<T, 1> &inY, const Vector<T, 2> &inZW);
-		Vector(const Vector<T, 1> &inX, const T &inY, const Vector<T, 2> &inZW);
-		Vector(const T &inX, const T &inY, const Vector<T, 2> &inZW);
-		Vector(const T &inX, const Vector<T, 1> &inY, const Vector<T, 2> &inZW);
-
-		Vector(const Vector<T, 2> &inXY, const Vector<T, 2> &inZW);
-
-		Vector(const Vector<T, 3> &inXYZ, const Vector<T, 1> &inW);
-		Vector(const Vector<T, 3> &inXYZ, const T &inW);
-
-		Vector(const Vector<T, 1> &inX, const Vector<T, 3> &inYZW);
-		Vector(const T &inX, const Vector<T, 3> &inYZW);
-
-		Vector(const Matrix<T, 4, 1> &rhs);
-
-
-		inline void
-		set(const T& x, const T& y, const T& z, const T& w);
-
-
-		inline void
-		setX(const T& value);
-
-		inline void
-		setY(const T& value);
-
-		inline void
-		setZ(const T& value);
-
-		inline void
-		setW(const T& value);
-
-
-		inline const T&
-		getX() const;
-
-		inline const T&
-		getY() const;
-
-		inline const T&
-		getZ() const;
-
-		inline const T&
-		getW() const;
-
-
-		Vector& operator = (const Matrix<T, 4, 1> &rhs);
-
-		bool operator == (const Vector &rhs) const;
-		bool operator != (const Vector &rhs) const;
-		bool operator < (const Vector &rhs) const;
-		bool operator <= (const Vector &rhs) const;
-		bool operator > (const Vector &rhs) const;
-		bool operator >= (const Vector &rhs) const;
-
-		const T& operator [] (uint8_t index) const;
-		T& operator [] (uint8_t index);
-		T* ptr();
-		const T* ptr() const;
-
-		Vector operator - () const;
-		Vector operator + (const Vector &rhs) const;
-		Vector operator - (const Vector &rhs) const;
-		T operator * (const Vector &rhs) const;
-		Vector operator * (const T &rhs) const;
-		Vector operator / (const T &rhs) const;
-
-		Vector& operator += (const Vector &rhs);
-		Vector& operator -= (const Vector &rhs);
-		Vector& operator *= (const T &rhs);
-		Vector& operator /= (const T &rhs);
-
-		float getLength() const;
-		float getLengthSquared() const;
-
-		void scale(float newLength);
-		Vector scaled(float newLength) const;
-
-		void normalize();
-		Vector normalized() const;
-
-		Matrix<T, 4, 1>&
-		asMatrix();
-
-		const Matrix<T, 4, 1>&
-		asMatrix() const;
-
-		Matrix<T, 1, 4>&
-		asTransposedMatrix();
-
-		const Matrix<T, 1, 4>&
-		asTransposedMatrix() const;
-
-	#ifndef __DOXYGEN__
-		IMPLEMENT_VECTOR_ACCESSOR2(x,x)	IMPLEMENT_VECTOR_ACCESSOR2(x,y)	IMPLEMENT_VECTOR_ACCESSOR2(x,z)	IMPLEMENT_VECTOR_ACCESSOR2(x,w)
-		IMPLEMENT_VECTOR_ACCESSOR2(y,x)	IMPLEMENT_VECTOR_ACCESSOR2(y,y)	IMPLEMENT_VECTOR_ACCESSOR2(y,z)	IMPLEMENT_VECTOR_ACCESSOR2(y,w)
-		IMPLEMENT_VECTOR_ACCESSOR2(z,x)	IMPLEMENT_VECTOR_ACCESSOR2(z,y)	IMPLEMENT_VECTOR_ACCESSOR2(z,z)	IMPLEMENT_VECTOR_ACCESSOR2(z,w)
-		IMPLEMENT_VECTOR_ACCESSOR2(w,x)	IMPLEMENT_VECTOR_ACCESSOR2(w,y)	IMPLEMENT_VECTOR_ACCESSOR2(w,z)	IMPLEMENT_VECTOR_ACCESSOR2(w,w)
-
-		IMPLEMENT_VECTOR_ACCESSOR3(x,x,x) IMPLEMENT_VECTOR_ACCESSOR3(x,x,y) IMPLEMENT_VECTOR_ACCESSOR3(x,x,z) IMPLEMENT_VECTOR_ACCESSOR3(x,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR3(x,y,x) IMPLEMENT_VECTOR_ACCESSOR3(x,y,y) IMPLEMENT_VECTOR_ACCESSOR3(x,y,z) IMPLEMENT_VECTOR_ACCESSOR3(x,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR3(x,z,x) IMPLEMENT_VECTOR_ACCESSOR3(x,z,y) IMPLEMENT_VECTOR_ACCESSOR3(x,z,z) IMPLEMENT_VECTOR_ACCESSOR3(x,z,w)
-		IMPLEMENT_VECTOR_ACCESSOR3(y,x,x) IMPLEMENT_VECTOR_ACCESSOR3(y,x,y) IMPLEMENT_VECTOR_ACCESSOR3(y,x,z) IMPLEMENT_VECTOR_ACCESSOR3(y,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR3(y,y,x) IMPLEMENT_VECTOR_ACCESSOR3(y,y,y) IMPLEMENT_VECTOR_ACCESSOR3(y,y,z) IMPLEMENT_VECTOR_ACCESSOR3(y,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR3(y,z,x) IMPLEMENT_VECTOR_ACCESSOR3(y,z,y) IMPLEMENT_VECTOR_ACCESSOR3(y,z,z) IMPLEMENT_VECTOR_ACCESSOR3(y,z,w)
-		IMPLEMENT_VECTOR_ACCESSOR3(z,x,x) IMPLEMENT_VECTOR_ACCESSOR3(z,x,y) IMPLEMENT_VECTOR_ACCESSOR3(z,x,z) IMPLEMENT_VECTOR_ACCESSOR3(z,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR3(z,y,x) IMPLEMENT_VECTOR_ACCESSOR3(z,y,y) IMPLEMENT_VECTOR_ACCESSOR3(z,y,z) IMPLEMENT_VECTOR_ACCESSOR3(z,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR3(z,z,x) IMPLEMENT_VECTOR_ACCESSOR3(z,z,y) IMPLEMENT_VECTOR_ACCESSOR3(z,z,z) IMPLEMENT_VECTOR_ACCESSOR3(z,z,w)
-		IMPLEMENT_VECTOR_ACCESSOR3(w,x,x) IMPLEMENT_VECTOR_ACCESSOR3(w,x,y) IMPLEMENT_VECTOR_ACCESSOR3(w,x,z) IMPLEMENT_VECTOR_ACCESSOR3(w,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR3(w,y,x) IMPLEMENT_VECTOR_ACCESSOR3(w,y,y) IMPLEMENT_VECTOR_ACCESSOR3(w,y,z) IMPLEMENT_VECTOR_ACCESSOR3(w,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR3(w,z,x) IMPLEMENT_VECTOR_ACCESSOR3(w,z,y) IMPLEMENT_VECTOR_ACCESSOR3(w,z,z) IMPLEMENT_VECTOR_ACCESSOR3(w,z,w)
-
-		IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,x) IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,y) IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,z) IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,x) IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,y) IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,z) IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(x,x,z,x) IMPLEMENT_VECTOR_ACCESSOR4(x,x,z,y) IMPLEMENT_VECTOR_ACCESSOR4(x,x,z,z) IMPLEMENT_VECTOR_ACCESSOR4(x,x,z,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,x) IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,y) IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,z) IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,x) IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,y) IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,z) IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(x,y,z,x) IMPLEMENT_VECTOR_ACCESSOR4(x,y,z,y) IMPLEMENT_VECTOR_ACCESSOR4(x,y,z,z) IMPLEMENT_VECTOR_ACCESSOR4(x,y,z,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(x,z,x,x) IMPLEMENT_VECTOR_ACCESSOR4(x,z,x,y) IMPLEMENT_VECTOR_ACCESSOR4(x,z,x,z) IMPLEMENT_VECTOR_ACCESSOR4(x,z,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(x,z,y,x) IMPLEMENT_VECTOR_ACCESSOR4(x,z,y,y) IMPLEMENT_VECTOR_ACCESSOR4(x,z,y,z) IMPLEMENT_VECTOR_ACCESSOR4(x,z,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(x,z,z,x) IMPLEMENT_VECTOR_ACCESSOR4(x,z,z,y) IMPLEMENT_VECTOR_ACCESSOR4(x,z,z,z) IMPLEMENT_VECTOR_ACCESSOR4(x,z,z,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(x,w,x,x) IMPLEMENT_VECTOR_ACCESSOR4(x,w,x,y) IMPLEMENT_VECTOR_ACCESSOR4(x,w,x,z) IMPLEMENT_VECTOR_ACCESSOR4(x,w,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(x,w,y,x) IMPLEMENT_VECTOR_ACCESSOR4(x,w,y,y) IMPLEMENT_VECTOR_ACCESSOR4(x,w,y,z) IMPLEMENT_VECTOR_ACCESSOR4(x,w,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(x,w,z,x) IMPLEMENT_VECTOR_ACCESSOR4(x,w,z,y) IMPLEMENT_VECTOR_ACCESSOR4(x,w,z,z) IMPLEMENT_VECTOR_ACCESSOR4(x,w,z,w)
-
-		IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,x) IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,y) IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,z) IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,x) IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,y) IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,z) IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(y,x,z,x) IMPLEMENT_VECTOR_ACCESSOR4(y,x,z,y) IMPLEMENT_VECTOR_ACCESSOR4(y,x,z,z) IMPLEMENT_VECTOR_ACCESSOR4(y,x,z,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,x) IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,y) IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,z) IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,x) IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,y) IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,z) IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(y,y,z,x) IMPLEMENT_VECTOR_ACCESSOR4(y,y,z,y) IMPLEMENT_VECTOR_ACCESSOR4(y,y,z,z) IMPLEMENT_VECTOR_ACCESSOR4(y,y,z,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(y,z,x,x) IMPLEMENT_VECTOR_ACCESSOR4(y,z,x,y) IMPLEMENT_VECTOR_ACCESSOR4(y,z,x,z) IMPLEMENT_VECTOR_ACCESSOR4(y,z,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(y,z,y,x) IMPLEMENT_VECTOR_ACCESSOR4(y,z,y,y) IMPLEMENT_VECTOR_ACCESSOR4(y,z,y,z) IMPLEMENT_VECTOR_ACCESSOR4(y,z,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(y,z,z,x) IMPLEMENT_VECTOR_ACCESSOR4(y,z,z,y) IMPLEMENT_VECTOR_ACCESSOR4(y,z,z,z) IMPLEMENT_VECTOR_ACCESSOR4(y,z,z,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(y,w,x,x) IMPLEMENT_VECTOR_ACCESSOR4(y,w,x,y) IMPLEMENT_VECTOR_ACCESSOR4(y,w,x,z) IMPLEMENT_VECTOR_ACCESSOR4(y,w,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(y,w,y,x) IMPLEMENT_VECTOR_ACCESSOR4(y,w,y,y) IMPLEMENT_VECTOR_ACCESSOR4(y,w,y,z) IMPLEMENT_VECTOR_ACCESSOR4(y,w,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(y,w,z,x) IMPLEMENT_VECTOR_ACCESSOR4(y,w,z,y) IMPLEMENT_VECTOR_ACCESSOR4(y,w,z,z) IMPLEMENT_VECTOR_ACCESSOR4(y,w,z,w)
-
-		IMPLEMENT_VECTOR_ACCESSOR4(z,x,x,x) IMPLEMENT_VECTOR_ACCESSOR4(z,x,x,y) IMPLEMENT_VECTOR_ACCESSOR4(z,x,x,z) IMPLEMENT_VECTOR_ACCESSOR4(z,x,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(z,x,y,x) IMPLEMENT_VECTOR_ACCESSOR4(z,x,y,y) IMPLEMENT_VECTOR_ACCESSOR4(z,x,y,z) IMPLEMENT_VECTOR_ACCESSOR4(z,x,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(z,x,z,x) IMPLEMENT_VECTOR_ACCESSOR4(z,x,z,y) IMPLEMENT_VECTOR_ACCESSOR4(z,x,z,z) IMPLEMENT_VECTOR_ACCESSOR4(z,x,z,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(z,y,x,x) IMPLEMENT_VECTOR_ACCESSOR4(z,y,x,y) IMPLEMENT_VECTOR_ACCESSOR4(z,y,x,z) IMPLEMENT_VECTOR_ACCESSOR4(z,y,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(z,y,y,x) IMPLEMENT_VECTOR_ACCESSOR4(z,y,y,y) IMPLEMENT_VECTOR_ACCESSOR4(z,y,y,z) IMPLEMENT_VECTOR_ACCESSOR4(z,y,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(z,y,z,x) IMPLEMENT_VECTOR_ACCESSOR4(z,y,z,y) IMPLEMENT_VECTOR_ACCESSOR4(z,y,z,z) IMPLEMENT_VECTOR_ACCESSOR4(z,y,z,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(z,z,x,x) IMPLEMENT_VECTOR_ACCESSOR4(z,z,x,y) IMPLEMENT_VECTOR_ACCESSOR4(z,z,x,z) IMPLEMENT_VECTOR_ACCESSOR4(z,z,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(z,z,y,x) IMPLEMENT_VECTOR_ACCESSOR4(z,z,y,y) IMPLEMENT_VECTOR_ACCESSOR4(z,z,y,z) IMPLEMENT_VECTOR_ACCESSOR4(z,z,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(z,z,z,x) IMPLEMENT_VECTOR_ACCESSOR4(z,z,z,y) IMPLEMENT_VECTOR_ACCESSOR4(z,z,z,z) IMPLEMENT_VECTOR_ACCESSOR4(z,z,z,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(z,w,x,x) IMPLEMENT_VECTOR_ACCESSOR4(z,w,x,y) IMPLEMENT_VECTOR_ACCESSOR4(z,w,x,z) IMPLEMENT_VECTOR_ACCESSOR4(z,w,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(z,w,y,x) IMPLEMENT_VECTOR_ACCESSOR4(z,w,y,y) IMPLEMENT_VECTOR_ACCESSOR4(z,w,y,z) IMPLEMENT_VECTOR_ACCESSOR4(z,w,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(z,w,z,x) IMPLEMENT_VECTOR_ACCESSOR4(z,w,z,y) IMPLEMENT_VECTOR_ACCESSOR4(z,w,z,z) IMPLEMENT_VECTOR_ACCESSOR4(z,w,z,w)
-
-		IMPLEMENT_VECTOR_ACCESSOR4(w,x,x,x) IMPLEMENT_VECTOR_ACCESSOR4(w,x,x,y) IMPLEMENT_VECTOR_ACCESSOR4(w,x,x,z) IMPLEMENT_VECTOR_ACCESSOR4(w,x,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(w,x,y,x) IMPLEMENT_VECTOR_ACCESSOR4(w,x,y,y) IMPLEMENT_VECTOR_ACCESSOR4(w,x,y,z) IMPLEMENT_VECTOR_ACCESSOR4(w,x,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(w,x,z,x) IMPLEMENT_VECTOR_ACCESSOR4(w,x,z,y) IMPLEMENT_VECTOR_ACCESSOR4(w,x,z,z) IMPLEMENT_VECTOR_ACCESSOR4(w,x,z,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(w,y,x,x) IMPLEMENT_VECTOR_ACCESSOR4(w,y,x,y) IMPLEMENT_VECTOR_ACCESSOR4(w,y,x,z) IMPLEMENT_VECTOR_ACCESSOR4(w,y,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(w,y,y,x) IMPLEMENT_VECTOR_ACCESSOR4(w,y,y,y) IMPLEMENT_VECTOR_ACCESSOR4(w,y,y,z) IMPLEMENT_VECTOR_ACCESSOR4(w,y,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(w,y,z,x) IMPLEMENT_VECTOR_ACCESSOR4(w,y,z,y) IMPLEMENT_VECTOR_ACCESSOR4(w,y,z,z) IMPLEMENT_VECTOR_ACCESSOR4(w,y,z,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(w,z,x,x) IMPLEMENT_VECTOR_ACCESSOR4(w,z,x,y) IMPLEMENT_VECTOR_ACCESSOR4(w,z,x,z) IMPLEMENT_VECTOR_ACCESSOR4(w,z,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(w,z,y,x) IMPLEMENT_VECTOR_ACCESSOR4(w,z,y,y) IMPLEMENT_VECTOR_ACCESSOR4(w,z,y,z) IMPLEMENT_VECTOR_ACCESSOR4(w,z,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(w,z,z,x) IMPLEMENT_VECTOR_ACCESSOR4(w,z,z,y) IMPLEMENT_VECTOR_ACCESSOR4(w,z,z,z) IMPLEMENT_VECTOR_ACCESSOR4(w,z,z,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(w,w,x,x) IMPLEMENT_VECTOR_ACCESSOR4(w,w,x,y) IMPLEMENT_VECTOR_ACCESSOR4(w,w,x,z) IMPLEMENT_VECTOR_ACCESSOR4(w,w,x,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(w,w,y,x) IMPLEMENT_VECTOR_ACCESSOR4(w,w,y,y) IMPLEMENT_VECTOR_ACCESSOR4(w,w,y,z) IMPLEMENT_VECTOR_ACCESSOR4(w,w,y,w)
-		IMPLEMENT_VECTOR_ACCESSOR4(w,w,z,x) IMPLEMENT_VECTOR_ACCESSOR4(w,w,z,y) IMPLEMENT_VECTOR_ACCESSOR4(w,w,z,z) IMPLEMENT_VECTOR_ACCESSOR4(w,w,z,w)
-	#endif
-
-	public:
-		T x;
-		T y;
-		T z;
-		T w;
-	};
-
-	template<typename U, typename T>
-	static inline Vector<T,4> operator * (const U &lhs, const Vector<T,4> &rhs)
-	{
-		return rhs * lhs;
+	void setZ(T z) { this->z = z; }
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	void setZ(U z) { this->z = std::round(z); }
+
+
+	void setW(T w) { this->w = w; }
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	void setW(U w) { this->w = std::round(w); }
+
+
+	void set(T x, T y, T z, T w) {
+		this->x = x;
+		this->y = y;
+		this->z = z;
+		this->w = w;
+	}
+
+	template<typename U>
+	requires std::integral<T> && std::floating_point<U>
+	void set(U x, U y, U z, U w) {
+		this->x = std::round(x);
+		this->y = std::round(y);
+		this->z = std::round(z);
+		this->w = std::round(w);
 	}
 
-	template<typename T, typename U>
-	static inline Vector<U,4> operator * (const Matrix<T, 4, 4> &lhs, const Vector<U,4> &rhs)
-	{
-		return lhs * rhs.asTMatrix();
+	// accessors
+	T& operator [] (std::size_t index)
+	{ return reinterpret_cast<T*>(this)[index]; }
+
+	const T& operator [] (std::size_t index) const
+	{ return reinterpret_cast<const T*>(this)[index]; }
+
+	T* ptr() { return reinterpret_cast<T*>(this); }
+
+	const T* ptr() const { return reinterpret_cast<const T*>(this); }
+
+	// operators
+	auto operator<=>(const Vector&) const = default;
+
+	constexpr Vector operator+ (const Vector &rhs) const
+	{ return Vector(x + rhs.x, y + rhs.y, z + rhs.z, w + rhs.w); }
+
+	constexpr Vector operator- (const Vector &rhs) const
+	{ return Vector(x - rhs.x, y - rhs.y, z - rhs.z, w - rhs.w); }
+
+	constexpr T operator* (const Vector &rhs) const
+	{ return x * rhs.x + y * rhs.y + z * rhs.z + w * rhs.w; }
+
+	// IMPLEMENT operator^ ?
+
+	template <typename U>
+	constexpr Vector operator* (U scale) const
+	{ return Vector<U, 4>(x * scale, y * scale, z * scale, w * scale); }
+
+	template <typename U>
+	constexpr Vector operator/ (U scale) const
+	{ return Vector<U, 4>(x / scale, y / scale, z / scale, w / scale); }
+
+	Vector& operator+= (const Vector &rhs)
+	{ x += rhs.x; y += rhs.y; z += rhs.z; w+= rhs.w; return *this; }
+
+	Vector& operator-= (const Vector &rhs)
+	{ x -= rhs.x; y -= rhs.y; z -= rhs.z; w-= rhs.w; return *this; }
+
+	Vector& operator *= (T scale)
+	{ x *= scale; y *= scale; z *= scale; w *= scale; return *this; }
+
+	Vector& operator /= (T scale)
+	{ x /= scale; y /= scale; z /= scale; w /= scale; return *this; }
+
+	// template <typename>
+	// requires std::is_signed<T>::value
+	constexpr Vector operator- () const
+	{ return Vector(-x, -y, -z, -w); }
+
+	// additional methods
+	T getLength() const
+	{ return std::sqrt(getLengthSquared()); }
+
+	T getLength() const
+	requires std::integral<T>
+	{ return round(std::sqrt(getLengthSquared())); }
+
+	WideWideType getLengthSquared() const
+	{ return std::pow(x, 2) + std::pow(y, 2) + std::pow(z, 2) + std::pow(w, 2); }
+
+	void scale(float newLength) { *this = scaled(newLength); }
+
+	Vector scaled(float newLength) const {
+		float scale = newLength / getLength();
+		return *this * scale;
 	}
 
-	typedef Vector<float, 4> 	Vector4f;
-	typedef Vector<int16_t, 4>	Vector4i;
-	typedef Vector<uint16_t, 4>	Vector4u;
+	void normalize() { scale(1.0f); }
+	Vector normalized() const { return scaled(1.0); }
+
+	bool hasNan() const { return std::isnan(x) || std::isnan(y) || std::isnan(z) || std::isnan(w); }
+	bool hasInf() const { return std::isinf(x) || std::isinf(y) || std::isinf(z) || std::isinf(w); }
+
+#ifndef __DOXYGEN__
+	IMPLEMENT_VECTOR_ACCESSOR2(x,x)	IMPLEMENT_VECTOR_ACCESSOR2(x,y)	IMPLEMENT_VECTOR_ACCESSOR2(x,z)	IMPLEMENT_VECTOR_ACCESSOR2(x,w)
+	IMPLEMENT_VECTOR_ACCESSOR2(y,x)	IMPLEMENT_VECTOR_ACCESSOR2(y,y)	IMPLEMENT_VECTOR_ACCESSOR2(y,z)	IMPLEMENT_VECTOR_ACCESSOR2(y,w)
+	IMPLEMENT_VECTOR_ACCESSOR2(z,x)	IMPLEMENT_VECTOR_ACCESSOR2(z,y)	IMPLEMENT_VECTOR_ACCESSOR2(z,z)	IMPLEMENT_VECTOR_ACCESSOR2(z,w)
+	IMPLEMENT_VECTOR_ACCESSOR2(w,x)	IMPLEMENT_VECTOR_ACCESSOR2(w,y)	IMPLEMENT_VECTOR_ACCESSOR2(w,z)	IMPLEMENT_VECTOR_ACCESSOR2(w,w)
+
+	IMPLEMENT_VECTOR_ACCESSOR3(x,x,x) IMPLEMENT_VECTOR_ACCESSOR3(x,x,y) IMPLEMENT_VECTOR_ACCESSOR3(x,x,z) IMPLEMENT_VECTOR_ACCESSOR3(x,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR3(x,y,x) IMPLEMENT_VECTOR_ACCESSOR3(x,y,y) IMPLEMENT_VECTOR_ACCESSOR3(x,y,z) IMPLEMENT_VECTOR_ACCESSOR3(x,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR3(x,z,x) IMPLEMENT_VECTOR_ACCESSOR3(x,z,y) IMPLEMENT_VECTOR_ACCESSOR3(x,z,z) IMPLEMENT_VECTOR_ACCESSOR3(x,z,w)
+	IMPLEMENT_VECTOR_ACCESSOR3(y,x,x) IMPLEMENT_VECTOR_ACCESSOR3(y,x,y) IMPLEMENT_VECTOR_ACCESSOR3(y,x,z) IMPLEMENT_VECTOR_ACCESSOR3(y,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR3(y,y,x) IMPLEMENT_VECTOR_ACCESSOR3(y,y,y) IMPLEMENT_VECTOR_ACCESSOR3(y,y,z) IMPLEMENT_VECTOR_ACCESSOR3(y,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR3(y,z,x) IMPLEMENT_VECTOR_ACCESSOR3(y,z,y) IMPLEMENT_VECTOR_ACCESSOR3(y,z,z) IMPLEMENT_VECTOR_ACCESSOR3(y,z,w)
+	IMPLEMENT_VECTOR_ACCESSOR3(z,x,x) IMPLEMENT_VECTOR_ACCESSOR3(z,x,y) IMPLEMENT_VECTOR_ACCESSOR3(z,x,z) IMPLEMENT_VECTOR_ACCESSOR3(z,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR3(z,y,x) IMPLEMENT_VECTOR_ACCESSOR3(z,y,y) IMPLEMENT_VECTOR_ACCESSOR3(z,y,z) IMPLEMENT_VECTOR_ACCESSOR3(z,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR3(z,z,x) IMPLEMENT_VECTOR_ACCESSOR3(z,z,y) IMPLEMENT_VECTOR_ACCESSOR3(z,z,z) IMPLEMENT_VECTOR_ACCESSOR3(z,z,w)
+	IMPLEMENT_VECTOR_ACCESSOR3(w,x,x) IMPLEMENT_VECTOR_ACCESSOR3(w,x,y) IMPLEMENT_VECTOR_ACCESSOR3(w,x,z) IMPLEMENT_VECTOR_ACCESSOR3(w,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR3(w,y,x) IMPLEMENT_VECTOR_ACCESSOR3(w,y,y) IMPLEMENT_VECTOR_ACCESSOR3(w,y,z) IMPLEMENT_VECTOR_ACCESSOR3(w,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR3(w,z,x) IMPLEMENT_VECTOR_ACCESSOR3(w,z,y) IMPLEMENT_VECTOR_ACCESSOR3(w,z,z) IMPLEMENT_VECTOR_ACCESSOR3(w,z,w)
+
+	IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,x) IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,y) IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,z) IMPLEMENT_VECTOR_ACCESSOR4(x,x,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,x) IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,y) IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,z) IMPLEMENT_VECTOR_ACCESSOR4(x,x,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(x,x,z,x) IMPLEMENT_VECTOR_ACCESSOR4(x,x,z,y) IMPLEMENT_VECTOR_ACCESSOR4(x,x,z,z) IMPLEMENT_VECTOR_ACCESSOR4(x,x,z,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,x) IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,y) IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,z) IMPLEMENT_VECTOR_ACCESSOR4(x,y,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,x) IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,y) IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,z) IMPLEMENT_VECTOR_ACCESSOR4(x,y,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(x,y,z,x) IMPLEMENT_VECTOR_ACCESSOR4(x,y,z,y) IMPLEMENT_VECTOR_ACCESSOR4(x,y,z,z) IMPLEMENT_VECTOR_ACCESSOR4(x,y,z,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(x,z,x,x) IMPLEMENT_VECTOR_ACCESSOR4(x,z,x,y) IMPLEMENT_VECTOR_ACCESSOR4(x,z,x,z) IMPLEMENT_VECTOR_ACCESSOR4(x,z,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(x,z,y,x) IMPLEMENT_VECTOR_ACCESSOR4(x,z,y,y) IMPLEMENT_VECTOR_ACCESSOR4(x,z,y,z) IMPLEMENT_VECTOR_ACCESSOR4(x,z,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(x,z,z,x) IMPLEMENT_VECTOR_ACCESSOR4(x,z,z,y) IMPLEMENT_VECTOR_ACCESSOR4(x,z,z,z) IMPLEMENT_VECTOR_ACCESSOR4(x,z,z,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(x,w,x,x) IMPLEMENT_VECTOR_ACCESSOR4(x,w,x,y) IMPLEMENT_VECTOR_ACCESSOR4(x,w,x,z) IMPLEMENT_VECTOR_ACCESSOR4(x,w,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(x,w,y,x) IMPLEMENT_VECTOR_ACCESSOR4(x,w,y,y) IMPLEMENT_VECTOR_ACCESSOR4(x,w,y,z) IMPLEMENT_VECTOR_ACCESSOR4(x,w,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(x,w,z,x) IMPLEMENT_VECTOR_ACCESSOR4(x,w,z,y) IMPLEMENT_VECTOR_ACCESSOR4(x,w,z,z) IMPLEMENT_VECTOR_ACCESSOR4(x,w,z,w)
+
+	IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,x) IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,y) IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,z) IMPLEMENT_VECTOR_ACCESSOR4(y,x,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,x) IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,y) IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,z) IMPLEMENT_VECTOR_ACCESSOR4(y,x,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(y,x,z,x) IMPLEMENT_VECTOR_ACCESSOR4(y,x,z,y) IMPLEMENT_VECTOR_ACCESSOR4(y,x,z,z) IMPLEMENT_VECTOR_ACCESSOR4(y,x,z,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,x) IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,y) IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,z) IMPLEMENT_VECTOR_ACCESSOR4(y,y,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,x) IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,y) IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,z) IMPLEMENT_VECTOR_ACCESSOR4(y,y,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(y,y,z,x) IMPLEMENT_VECTOR_ACCESSOR4(y,y,z,y) IMPLEMENT_VECTOR_ACCESSOR4(y,y,z,z) IMPLEMENT_VECTOR_ACCESSOR4(y,y,z,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(y,z,x,x) IMPLEMENT_VECTOR_ACCESSOR4(y,z,x,y) IMPLEMENT_VECTOR_ACCESSOR4(y,z,x,z) IMPLEMENT_VECTOR_ACCESSOR4(y,z,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(y,z,y,x) IMPLEMENT_VECTOR_ACCESSOR4(y,z,y,y) IMPLEMENT_VECTOR_ACCESSOR4(y,z,y,z) IMPLEMENT_VECTOR_ACCESSOR4(y,z,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(y,z,z,x) IMPLEMENT_VECTOR_ACCESSOR4(y,z,z,y) IMPLEMENT_VECTOR_ACCESSOR4(y,z,z,z) IMPLEMENT_VECTOR_ACCESSOR4(y,z,z,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(y,w,x,x) IMPLEMENT_VECTOR_ACCESSOR4(y,w,x,y) IMPLEMENT_VECTOR_ACCESSOR4(y,w,x,z) IMPLEMENT_VECTOR_ACCESSOR4(y,w,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(y,w,y,x) IMPLEMENT_VECTOR_ACCESSOR4(y,w,y,y) IMPLEMENT_VECTOR_ACCESSOR4(y,w,y,z) IMPLEMENT_VECTOR_ACCESSOR4(y,w,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(y,w,z,x) IMPLEMENT_VECTOR_ACCESSOR4(y,w,z,y) IMPLEMENT_VECTOR_ACCESSOR4(y,w,z,z) IMPLEMENT_VECTOR_ACCESSOR4(y,w,z,w)
+
+	IMPLEMENT_VECTOR_ACCESSOR4(z,x,x,x) IMPLEMENT_VECTOR_ACCESSOR4(z,x,x,y) IMPLEMENT_VECTOR_ACCESSOR4(z,x,x,z) IMPLEMENT_VECTOR_ACCESSOR4(z,x,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(z,x,y,x) IMPLEMENT_VECTOR_ACCESSOR4(z,x,y,y) IMPLEMENT_VECTOR_ACCESSOR4(z,x,y,z) IMPLEMENT_VECTOR_ACCESSOR4(z,x,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(z,x,z,x) IMPLEMENT_VECTOR_ACCESSOR4(z,x,z,y) IMPLEMENT_VECTOR_ACCESSOR4(z,x,z,z) IMPLEMENT_VECTOR_ACCESSOR4(z,x,z,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(z,y,x,x) IMPLEMENT_VECTOR_ACCESSOR4(z,y,x,y) IMPLEMENT_VECTOR_ACCESSOR4(z,y,x,z) IMPLEMENT_VECTOR_ACCESSOR4(z,y,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(z,y,y,x) IMPLEMENT_VECTOR_ACCESSOR4(z,y,y,y) IMPLEMENT_VECTOR_ACCESSOR4(z,y,y,z) IMPLEMENT_VECTOR_ACCESSOR4(z,y,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(z,y,z,x) IMPLEMENT_VECTOR_ACCESSOR4(z,y,z,y) IMPLEMENT_VECTOR_ACCESSOR4(z,y,z,z) IMPLEMENT_VECTOR_ACCESSOR4(z,y,z,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(z,z,x,x) IMPLEMENT_VECTOR_ACCESSOR4(z,z,x,y) IMPLEMENT_VECTOR_ACCESSOR4(z,z,x,z) IMPLEMENT_VECTOR_ACCESSOR4(z,z,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(z,z,y,x) IMPLEMENT_VECTOR_ACCESSOR4(z,z,y,y) IMPLEMENT_VECTOR_ACCESSOR4(z,z,y,z) IMPLEMENT_VECTOR_ACCESSOR4(z,z,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(z,z,z,x) IMPLEMENT_VECTOR_ACCESSOR4(z,z,z,y) IMPLEMENT_VECTOR_ACCESSOR4(z,z,z,z) IMPLEMENT_VECTOR_ACCESSOR4(z,z,z,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(z,w,x,x) IMPLEMENT_VECTOR_ACCESSOR4(z,w,x,y) IMPLEMENT_VECTOR_ACCESSOR4(z,w,x,z) IMPLEMENT_VECTOR_ACCESSOR4(z,w,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(z,w,y,x) IMPLEMENT_VECTOR_ACCESSOR4(z,w,y,y) IMPLEMENT_VECTOR_ACCESSOR4(z,w,y,z) IMPLEMENT_VECTOR_ACCESSOR4(z,w,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(z,w,z,x) IMPLEMENT_VECTOR_ACCESSOR4(z,w,z,y) IMPLEMENT_VECTOR_ACCESSOR4(z,w,z,z) IMPLEMENT_VECTOR_ACCESSOR4(z,w,z,w)
+
+	IMPLEMENT_VECTOR_ACCESSOR4(w,x,x,x) IMPLEMENT_VECTOR_ACCESSOR4(w,x,x,y) IMPLEMENT_VECTOR_ACCESSOR4(w,x,x,z) IMPLEMENT_VECTOR_ACCESSOR4(w,x,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(w,x,y,x) IMPLEMENT_VECTOR_ACCESSOR4(w,x,y,y) IMPLEMENT_VECTOR_ACCESSOR4(w,x,y,z) IMPLEMENT_VECTOR_ACCESSOR4(w,x,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(w,x,z,x) IMPLEMENT_VECTOR_ACCESSOR4(w,x,z,y) IMPLEMENT_VECTOR_ACCESSOR4(w,x,z,z) IMPLEMENT_VECTOR_ACCESSOR4(w,x,z,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(w,y,x,x) IMPLEMENT_VECTOR_ACCESSOR4(w,y,x,y) IMPLEMENT_VECTOR_ACCESSOR4(w,y,x,z) IMPLEMENT_VECTOR_ACCESSOR4(w,y,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(w,y,y,x) IMPLEMENT_VECTOR_ACCESSOR4(w,y,y,y) IMPLEMENT_VECTOR_ACCESSOR4(w,y,y,z) IMPLEMENT_VECTOR_ACCESSOR4(w,y,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(w,y,z,x) IMPLEMENT_VECTOR_ACCESSOR4(w,y,z,y) IMPLEMENT_VECTOR_ACCESSOR4(w,y,z,z) IMPLEMENT_VECTOR_ACCESSOR4(w,y,z,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(w,z,x,x) IMPLEMENT_VECTOR_ACCESSOR4(w,z,x,y) IMPLEMENT_VECTOR_ACCESSOR4(w,z,x,z) IMPLEMENT_VECTOR_ACCESSOR4(w,z,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(w,z,y,x) IMPLEMENT_VECTOR_ACCESSOR4(w,z,y,y) IMPLEMENT_VECTOR_ACCESSOR4(w,z,y,z) IMPLEMENT_VECTOR_ACCESSOR4(w,z,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(w,z,z,x) IMPLEMENT_VECTOR_ACCESSOR4(w,z,z,y) IMPLEMENT_VECTOR_ACCESSOR4(w,z,z,z) IMPLEMENT_VECTOR_ACCESSOR4(w,z,z,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(w,w,x,x) IMPLEMENT_VECTOR_ACCESSOR4(w,w,x,y) IMPLEMENT_VECTOR_ACCESSOR4(w,w,x,z) IMPLEMENT_VECTOR_ACCESSOR4(w,w,x,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(w,w,y,x) IMPLEMENT_VECTOR_ACCESSOR4(w,w,y,y) IMPLEMENT_VECTOR_ACCESSOR4(w,w,y,z) IMPLEMENT_VECTOR_ACCESSOR4(w,w,y,w)
+	IMPLEMENT_VECTOR_ACCESSOR4(w,w,z,x) IMPLEMENT_VECTOR_ACCESSOR4(w,w,z,y) IMPLEMENT_VECTOR_ACCESSOR4(w,w,z,z) IMPLEMENT_VECTOR_ACCESSOR4(w,w,z,w)
+#endif
+
+	// depricated methods
+	template<typename U>
+	[[deprecated("Use common constructor instead!")]]
+	Vector<U, 4>
+	convert() const { return {*this}; }
+
+protected:
+	template<typename U>
+	friend IOStream&
+	operator<<(IOStream& os, const Vector<U, 4>& c);
+};
+
+template<typename U, typename T>
+static inline Vector<T,4> operator* (const U &lhs, const Vector<T,4> &rhs)
+{ return rhs * lhs; }
+
+// Should be covered by the above
+// template<typename T>
+// static inline Vector<T,4> operator* (const T &lhs, const Vector<T,4> &rhs)
+// {
+// 	return rhs * lhs;
+// }
+
+template<typename T, typename U>
+static inline Vector<U,4> operator* (const Matrix<T, 4, 4> &lhs, const Vector<U,4> &rhs)
+{ return lhs * rhs.asTMatrix(); }
+
+#if __has_include(<modm/io/iostream.hpp>)
+#include <modm/io/iostream.hpp>
+
+template<typename U>
+IOStream&
+operator<< (IOStream& os, const Vector<U, 4>& v) {
+	os << "x=" << v.x << "\ty=" << v.y << "\tz=" << v.z << "\tw=" << v.w;
+	return os;
 }
+#endif
 
-#include "vector4_impl.hpp"
+using Vector4f = Vector<float, 4>;
+using Vector4i = Vector<int16_t, 4>;
+using Vector4u = Vector<uint16_t, 4>;
 
-#endif // MODM_VECTOR4_HPP
+} // namespace modm
\ No newline at end of file
diff --git a/src/modm/math/geometry/vector4_impl.hpp b/src/modm/math/geometry/vector4_impl.hpp
deleted file mode 100644
index 36e8d2c664..0000000000
--- a/src/modm/math/geometry/vector4_impl.hpp
+++ /dev/null
@@ -1,788 +0,0 @@
-/*
- * Copyright (c) 2011-2012, Fabian Greif
- * Copyright (c) 2012, Niklas Hauser
- *
- * This file is part of the modm project.
- *
- * This Source Code Form is subject to the terms of the Mozilla Public
- * License, v. 2.0. If a copy of the MPL was not distributed with this
- * file, You can obtain one at http://mozilla.org/MPL/2.0/.
- */
-// ----------------------------------------------------------------------------
-
-#ifndef MODM_VECTOR4_HPP
-	#error	"Don't include this file directly, use 'vector4.hpp' instead!"
-#endif
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector()
-:
-	x(),
-	y(),
-	z(),
-	w()
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(T inX, T inY, T inZ, T inW)
-:
-	x(inX),
-	y(inY),
-	z(inZ),
-	w(inW)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 1> &inX, const modm::Vector<T, 1> &inY, const modm::Vector<T, 1> &inZ, const modm::Vector<T, 1> &inW)
-:
-	x(inX.x),
-	y(inY.x),
-	z(inZ.x),
-	w(inW.x)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 1> &inX, const modm::Vector<T, 1> &inY, const modm::Vector<T, 1> &inZ, const T &inW)
-:
-	x(inX.x),
-	y(inY.x),
-	z(inZ.x),
-	w(inW)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 1> &inX, const modm::Vector<T, 1> &inY, const T &inZ, const T &inW)
-:
-	x(inX.x),
-	y(inY.x),
-	z(inZ),
-	w(inW)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 1> &inX, const T &inY, const modm::Vector<T, 1> &inZ, const T &inW)
-:
-	x(inX.x),
-	y(inY),
-	z(inZ.x),
-	w(inW)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const T &inX, const modm::Vector<T, 1> &inY, const modm::Vector<T, 1> &inZ, const T &inW)
-:
-	x(inX),
-	y(inY.x),
-	z(inZ.x),
-	w(inW)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 1> &inX, const T &inY, const T &inZ, const T &inW)
-:
-	x(inX.x),
-	y(inY),
-	z(inZ),
-	w(inW)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const T &inX, const modm::Vector<T, 1> &inY, const T &inZ, const T &inW)
-:
-	x(inX),
-	y(inY.x),
-	z(inZ),
-	w(inW)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 1> &inX, const modm::Vector<T, 1> &inY, const T &inZ, const modm::Vector<T, 1> &inW)
-:
-	x(inX.x),
-	y(inY.x),
-	z(inZ),
-	w(inW.x)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 1> &inX, const T &inY, const T &inZ, const modm::Vector<T, 1> &inW)
-:
-	x(inX.x),
-	y(inY),
-	z(inZ),
-	w(inW.x)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const T &inX, const modm::Vector<T, 1> &inY, const T &inZ, const modm::Vector<T, 1> &inW)
-:
-	x(inX),
-	y(inY.x),
-	z(inZ),
-	w(inW.x)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const T &inX, const T &inY, const T &inZ, const modm::Vector<T, 1> &inW)
-:
-	x(inX),
-	y(inY),
-	z(inZ),
-	w(inW.x)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 1> &inX, const T &inY, const modm::Vector<T, 1> &inZ, const modm::Vector<T, 1> &inW)
-:
-	x(inX.x),
-	y(inY),
-	z(inZ.x),
-	w(inW.x)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const T &inX, const T &inY, const modm::Vector<T, 1> &inZ, const modm::Vector<T, 1> &inW)
-:
-	x(inX),
-	y(inY),
-	z(inZ.x),
-	w(inW.x)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const T &inX, const modm::Vector<T, 1> &inY, const modm::Vector<T, 1> &inZ, const modm::Vector<T, 1> &inW)
-:
-	x(inX),
-	y(inY.x),
-	z(inZ.x),
-	w(inW.x)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 2> &inXY, const modm::Vector<T, 1> &inZ, const modm::Vector<T, 1> &inW)
-:
-	x(inXY.x),
-	y(inXY.y),
-	z(inZ.x),
-	w(inW.x)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 2> &inXY, const modm::Vector<T, 1> &inZ, const T &inW)
-:
-	x(inXY.x),
-	y(inXY.y),
-	z(inZ.x),
-	w(inW)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 2> &inXY, const T &inZ, const T &inW)
-:
-	x(inXY.x),
-	y(inXY.y),
-	z(inZ),
-	w(inW)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 2> &inXY, const T &inZ, const modm::Vector<T, 1> &inW)
-:
-	x(inXY.x),
-	y(inXY.y),
-	z(inZ),
-	w(inW.x)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 1> &inX, const modm::Vector<T, 2> &inYZ, const modm::Vector<T, 1> &inW)
-:
-	x(inX.x),
-	y(inYZ.x),
-	z(inYZ.y),
-	w(inW.x)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 1> &inX, const modm::Vector<T, 2> &inYZ, const T &inW)
-:
-	x(inX.x),
-	y(inYZ.x),
-	z(inYZ.y),
-	w(inW)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const T &inX, const modm::Vector<T, 2> &inYZ, const T &inW)
-:
-	x(inX),
-	y(inYZ.x),
-	z(inYZ.y),
-	w(inW)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const T &inX, const modm::Vector<T, 2> &inYZ, const modm::Vector<T, 1> &inW)
-:
-	x(inX),
-	y(inYZ.x),
-	z(inYZ.y),
-	w(inW.x)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 1> &inX, const modm::Vector<T, 1> &inY, const modm::Vector<T, 2> &inZW)
-:
-	x(inX.x),
-	y(inY.x),
-	z(inZW.x),
-	w(inZW.y)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 1> &inX, const T &inY, const modm::Vector<T, 2> &inZW)
-:
-	x(inX.x),
-	y(inY),
-	z(inZW.x),
-	w(inZW.y)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const T &inX, const T &inY, const modm::Vector<T, 2> &inZW)
-:
-	x(inX),
-	y(inY),
-	z(inZW.x),
-	w(inZW.y)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const T &inX, const modm::Vector<T, 1> &inY, const modm::Vector<T, 2> &inZW)
-:
-	x(inX),
-	y(inY.x),
-	z(inZW.x),
-	w(inZW.y)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 2> &inXY, const modm::Vector<T, 2> &inZW)
-:
-	x(inXY.x),
-	y(inXY.y),
-	z(inZW.x),
-	w(inZW.y)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 3> &inXYZ, const modm::Vector<T, 1> &inW)
-:
-	x(inXYZ.x),
-	y(inXYZ.y),
-	z(inXYZ.z),
-	w(inW.x)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 3> &inXYZ, const T &inW)
-:
-	x(inXYZ.x),
-	y(inXYZ.y),
-	z(inXYZ.z),
-	w(inW)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Vector<T, 1> &inX, const modm::Vector<T, 3> &inYZW)
-:
-	x(inX.x),
-	y(inYZW.x),
-	z(inYZW.y),
-	w(inYZW.z)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const T &inX, const modm::Vector<T, 3> &inYZW)
-:
-	x(inX),
-	y(inYZW.x),
-	z(inYZW.y),
-	w(inYZW.z)
-{
-}
-
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(const modm::Matrix<T, 4, 1> &rhs)
-:
-	x(reinterpret_cast<const T*>(&rhs)[0]),
-	y(reinterpret_cast<const T*>(&rhs)[1]),
-	z(reinterpret_cast<const T*>(&rhs)[2]),
-	w(reinterpret_cast<const T*>(&rhs)[3])
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>::Vector(T inVal)
-:
-	x(inVal),
-	y(inVal),
-	z(inVal),
-	w(inVal)
-{
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-void
-modm::Vector<T, 4>::set(const T& x_, const T& y_, const T& z_, const T& w_)
-{
-	this->x = x_;
-	this->y = y_;
-	this->z = z_;
-	this->w = w_;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-void
-modm::Vector<T, 4>::setX(const T& value)
-{
-	this->x = value;
-}
-
-template<typename T>
-void
-modm::Vector<T, 4>::setY(const T& value)
-{
-	this->y = value;
-}
-
-template<typename T>
-void
-modm::Vector<T, 4>::setZ(const T& value)
-{
-	this->z = value;
-}
-
-template<typename T>
-void
-modm::Vector<T, 4>::setW(const T& value)
-{
-	this->w = value;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-const T&
-modm::Vector<T, 4>::getX() const
-{
-	return this->x;
-}
-
-template<typename T>
-const T&
-modm::Vector<T, 4>::getY() const
-{
-	return this->y;
-}
-
-template<typename T>
-const T&
-modm::Vector<T, 4>::getZ() const
-{
-	return this->z;
-}
-
-template<typename T>
-const T&
-modm::Vector<T, 4>::getW() const
-{
-	return this->w;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>&
-modm::Vector<T, 4>::operator = (const modm::Matrix<T, 4, 1> &rhs)
-{
-	x = reinterpret_cast<const T*>(&rhs)[0];
-	y = reinterpret_cast<const T*>(&rhs)[1];
-	z = reinterpret_cast<const T*>(&rhs)[2];
-	w = reinterpret_cast<const T*>(&rhs)[3];
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 4>::operator == (const modm::Vector<T, 4> &rhs) const
-{
-	return (rhs.x == x) && (rhs.y == y) && (rhs.z == z) && (rhs.w == w);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 4>::operator != (const modm::Vector<T, 4> &rhs) const
-{
-	return (rhs.x != x) || (rhs.y != y) || (rhs.z != z) || (rhs.w != w);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 4>::operator < (const modm::Vector<T, 4> &rhs) const
-{
-	return (x < rhs.x) || ((x == rhs.x) && ((y < rhs.y) || ((y == rhs.y) && ((z < rhs.z) || ((z == rhs.z) && (w < rhs.w))))));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 4>::operator <= (const modm::Vector<T, 4> &rhs) const
-{
-	return (x < rhs.x) || ((x == rhs.x) && ((y < rhs.y) || ((y == rhs.y) && ((z < rhs.z) || ((z == rhs.z) && (w <= rhs.w))))));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 4>::operator > (const modm::Vector<T, 4> &rhs) const
-{
-	return (x > rhs.x) || ((x == rhs.x) && ((y > rhs.y) || ((y == rhs.y) && ((z > rhs.z) || ((z == rhs.z) && (w > rhs.w))))));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-bool
-modm::Vector<T, 4>::operator >= (const modm::Vector<T, 4> &rhs) const
-{
-	return (x > rhs.x) || ((x == rhs.x) && ((y > rhs.y) || ((y == rhs.y) && ((z > rhs.z) || ((z == rhs.z) && (w >= rhs.w))))));
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-const T&
-modm::Vector<T, 4>::operator [] (uint8_t index) const
-{
-	return reinterpret_cast<const T*>(this)[index];
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-T&
-modm::Vector<T, 4>::operator [] (uint8_t index)
-{
-	return reinterpret_cast<T*>(this)[index];
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-T*
-modm::Vector<T, 4>::ptr()
-{
-	return reinterpret_cast<T*>(this);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-const T*
-modm::Vector<T, 4>::ptr() const
-{
-	return reinterpret_cast<const T*>(this);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>
-modm::Vector<T, 4>::operator + (const modm::Vector<T, 4> &rhs) const
-{
-	return modm::Vector<T, 4>(x+rhs.x, y+rhs.y, z+rhs.z, w+rhs.w);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>
-modm::Vector<T, 4>::operator - (const modm::Vector<T, 4> &rhs) const
-{
-	return modm::Vector<T, 4>(x-rhs.x, y-rhs.y, z-rhs.z, w-rhs.w);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-T
-modm::Vector<T, 4>::operator * (const modm::Vector<T, 4> &rhs) const
-{
-	return x*rhs.x + y*rhs.y + z*rhs.z + w*rhs.w;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>
-modm::Vector<T, 4>::operator * (const T &rhs) const
-{
-	return modm::Vector<T, 4>(x*rhs, y*rhs, z*rhs, w*rhs);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>
-modm::Vector<T, 4>::operator / (const T &rhs) const
-{
-	return modm::Vector<T, 4>(x/rhs, y/rhs, z/rhs, w/rhs);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>&
-modm::Vector<T, 4>::operator += (const modm::Vector<T, 4> &rhs)
-{
-	x += rhs.x;
-	y += rhs.y;
-	z += rhs.z;
-	w += rhs.w;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>&
-modm::Vector<T, 4>::operator -= (const modm::Vector<T, 4> &rhs)
-{
-	x -= rhs.x;
-	y -= rhs.y;
-	z -= rhs.z;
-	w -= rhs.w;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>&
-modm::Vector<T, 4>::operator *= (const T &rhs)
-{
-	x *= rhs;
-	y *= rhs;
-	z *= rhs;
-	w *= rhs;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>&
-modm::Vector<T, 4>::operator /= (const T &rhs)
-{
-	x /= rhs;
-	y /= rhs;
-	z /= rhs;
-	w /= rhs;
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>
-modm::Vector<T, 4>::operator - () const
-{
-	return modm::Vector<T, 4>(-x, -y, -z, -w);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-float
-modm::Vector<T, 4>::getLength() const
-{
-	return std::sqrt(getLengthSquared());
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-float
-modm::Vector<T, 4>::getLengthSquared() const
-{
-	return x*x + y*y + z*z + w*w;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-void
-modm::Vector<T, 4>::scale(float newLength)
-{
-	*this = scaled(newLength);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>
-modm::Vector<T, 4>::scaled(float newLength) const
-{
-	float scale = newLength / getLength();
-	return *this * scale;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-void
-modm::Vector<T, 4>::normalize()
-{
-	scale(1.0f);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Vector<T, 4>
-modm::Vector<T, 4>::normalized() const
-{
-	return scaled(1.0);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Matrix<T, 4, 1>&
-modm::Vector<T, 4>::asMatrix()
-{
-	return *(modm::Matrix<T, 4, 1>*)this;
-}
-
-template<typename T>
-const modm::Matrix<T, 4, 1>&
-modm::Vector<T, 4>::asMatrix() const
-{
-	return *(modm::Matrix<T, 4, 1>*)this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-modm::Matrix<T, 1, 4>&
-modm::Vector<T, 4>::asTransposedMatrix()
-{
-	return *(modm::Matrix<T, 1, 4>*)this;
-}
-
-
-template<typename T>
-const modm::Matrix<T, 1, 4>&
-modm::Vector<T, 4>::asTransposedMatrix() const
-{
-	return *(modm::Matrix<T, 1, 4>*)this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T>
-static inline modm::Vector<T,4>
-operator * (const T &lhs, const modm::Vector<T,4> &rhs)
-{
-	return rhs * lhs;
-}
-
-template<class T, class U>
-static inline modm::Vector<U,4>
-operator * (const modm::Matrix<T, 4, 4> &lhs, const modm::Vector<U,4> &rhs)
-{
-	return lhs * rhs.asTMatrix();
-}
-
diff --git a/src/modm/math/geometry/vector_impl.hpp b/src/modm/math/geometry/vector_impl.hpp
deleted file mode 100644
index ccb4d78451..0000000000
--- a/src/modm/math/geometry/vector_impl.hpp
+++ /dev/null
@@ -1,333 +0,0 @@
-/*
- * Copyright (c) 2011-2012, Fabian Greif
- * Copyright (c) 2012, Niklas Hauser
- *
- * This file is part of the modm project.
- *
- * This Source Code Form is subject to the terms of the Mozilla Public
- * License, v. 2.0. If a copy of the MPL was not distributed with this
- * file, You can obtain one at http://mozilla.org/MPL/2.0/.
- */
-// ----------------------------------------------------------------------------
-
-#ifndef MODM_VECTOR_HPP
-	#error	"Don't include this file directly, use 'vector.hpp' instead!"
-#endif
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-modm::Vector<T, N>::Vector()
-{
-}
-
-template<typename T, uint8_t N>
-modm::Vector<T, N>::Vector(const T *ptData)
-{
-	memcpy(coords, ptData, sizeof(T) * N);
-}
-
-template<typename T, uint8_t N>
-modm::Vector<T, N>::Vector(const modm::Matrix<T, N, 1> &rhs)
-{
-	memcpy(coords, &rhs, sizeof(T) * N);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-uint8_t
-modm::Vector<T, N>::getSize()
-{
-	return N;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-modm::Vector<T, N>&
-modm::Vector<T, N>::operator = (const modm::Matrix<T, N, 1> &rhs)
-{
-	memcpy(coords, &rhs, sizeof(T) * N);
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-bool
-modm::Vector<T, N>::operator == (const modm::Vector<T, N> &rhs) const
-{
-	return memcmp(coords, rhs.coords, sizeof(T)*N) == 0;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-bool
-modm::Vector<T, N>::operator != (const modm::Vector<T, N> &rhs) const
-{
-	return memcmp(coords, rhs.coords, sizeof(T)*N) != 0;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-bool
-modm::Vector<T, N>::operator < (const modm::Vector<T, N> &rhs) const
-{
-	for (uint_fast8_t i = 0; i < N; ++i)
-	{
-		if ((*this)[i] < rhs[i]) {
-			return true;
-		}
-		else if ((*this)[i] > rhs[i]) {
-			return false;
-		}
-	}
-	return false;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-bool
-modm::Vector<T, N>::operator <= (const modm::Vector<T, N> &rhs) const
-{
-	for (uint_fast8_t i = 0; i < N; ++i)
-	{
-		if ((*this)[i] < rhs[i]) {
-			return true;
-		}
-		else if ((*this)[i] > rhs[i]) {
-			return false;
-		}
-	}
-	return true;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-bool
-modm::Vector<T, N>::operator > (const modm::Vector<T, N> &rhs) const
-{
-	for (uint_fast8_t i = 0; i < N; ++i)
-	{
-		if ((*this)[i] > rhs[i]) {
-			return true;
-		}
-		else if ((*this)[i] < rhs[i]) {
-			return false;
-		}
-	}
-	return false;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-bool
-modm::Vector<T, N>::operator >= (const modm::Vector<T, N> &rhs) const
-{
-	for (uint_fast8_t i = 0; i < N; ++i)
-	{
-		if ((*this)[i] > rhs[i]) {
-			return true;
-		}
-		else if ((*this)[i] < rhs[i]) {
-			return false;
-		}
-	}
-	return true;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-const T&
-modm::Vector<T, N>::operator [] (uint8_t index) const
-{
-	return coords[index];
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-T&
-modm::Vector<T, N>::operator [] (uint8_t index)
-{
-	return coords[index];
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-T*
-modm::Vector<T, N>::ptr()
-{
-	return reinterpret_cast<T*>(this);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-const T*
-modm::Vector<T, N>::ptr() const
-{
-	return reinterpret_cast<const T*>(this);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-modm::Vector<T, N>
-modm::Vector<T, N>::operator + (const modm::Vector<T, N> &rhs) const
-{
-	modm::Vector<T, N> pt;
-	for (uint_fast8_t i = 0; i < N; ++i) {
-		pt[i] = coords[i] + rhs.coords[i];
-	}
-	return pt;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-modm::Vector<T, N>
-modm::Vector<T, N>::operator - (const modm::Vector<T, N> &rhs) const
-{
-	modm::Vector<T, N> pt;
-	for (uint_fast8_t i = 0; i < N; ++i) {
-		pt[i] = coords[i] - rhs.coords[i];
-	}
-	return pt;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-modm::Matrix<T, N, 1>&
-modm::Vector<T, N>::asMatrix()
-{
-	return *reinterpret_cast<modm::Matrix<T, N, 1>*>(this);
-}
-
-template<typename T, uint8_t N>
-const modm::Matrix<T, N, 1>&
-modm::Vector<T, N>::asMatrix() const
-{
-	return *reinterpret_cast<const modm::Matrix<T, N, 1>*>(this);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-const modm::Matrix<T, 1, N>&
-modm::Vector<T, N>::asTransposedMatrix() const
-{
-	return *reinterpret_cast<const modm::Matrix<T, 1, N>*>(this);
-}
-
-template<typename T, uint8_t N>
-modm::Matrix<T, 1, N>&
-modm::Vector<T, N>::asTransposedMatrix()
-{
-	return *reinterpret_cast<modm::Matrix<T, 1, N>*>(this);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-T
-modm::Vector<T, N>::operator * (const Vector &rhs) const
-{
-	T v = 0;
-	for (uint_fast8_t i = 0; i < N; ++i) {
-		v += (*this)[i]*rhs[i];
-	}
-
-	return v;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-modm::Vector<T, N>
-modm::Vector<T, N>::operator * (const T &rhs) const
-{
-	modm::Vector<T, N> pt;
-	for (uint_fast8_t i = 0; i < N; ++i) {
-		pt[i] = coords[i] * rhs;
-	}
-	return pt;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-modm::Vector<T, N>
-modm::Vector<T, N>::operator / (const T &rhs) const
-{
-	modm::Vector<T, N> pt;
-	for (uint_fast8_t i = 0; i < N; ++i) {
-		pt[i] = coords[i] / rhs;
-	}
-	return pt;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-modm::Vector<T, N>&
-modm::Vector<T, N>::operator += (const Vector &rhs)
-{
-	for (uint_fast8_t i = 0; i < N; ++i) {
-		coords[i] += rhs.coords[i];
-	}
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-modm::Vector<T, N>&
-modm::Vector<T, N>::operator -= (const Vector &rhs)
-{
-	for (uint_fast8_t i = 0; i < N; ++i) {
-		coords[i] -= rhs.coords[i];
-	}
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-modm::Vector<T, N>&
-modm::Vector<T, N>::operator *= (const T &rhs)
-{
-	for (uint_fast8_t i = 0; i < N; ++i) {
-		coords[i] -= rhs;
-	}
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-modm::Vector<T, N>&
-modm::Vector<T, N>::operator /= (const T &rhs)
-{
-	for (uint_fast8_t i = 0; i < N; ++i) {
-		coords[i] /= rhs;
-	}
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-modm::Vector<T, N>&
-modm::Vector<T, N>::operator - ()
-{
-	for (uint_fast8_t i = 0; i < N; ++i) {
-		coords[i] = -coords[i];
-	}
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-T
-modm::Vector<T, N>::getLength() const
-{
-	return std::sqrt(getLengthSquared());
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
-T
-modm::Vector<T, N>::getLengthSquared() const
-{
-	T len2 = 0;
-	for (uint_fast8_t i = 0; i < N; ++i) {
-		len2 += (*this)[i]*(*this)[i];
-	}
-
-	return len2;
-}
diff --git a/src/modm/math/lu_decomposition.hpp b/src/modm/math/lu_decomposition.hpp
index eb1c18a29a..c2b618d416 100644
--- a/src/modm/math/lu_decomposition.hpp
+++ b/src/modm/math/lu_decomposition.hpp
@@ -11,8 +11,7 @@
  */
 // ----------------------------------------------------------------------------
 
-#ifndef MODM_LU_DECOMPOSITION_HPP
-#define MODM_LU_DECOMPOSITION_HPP
+#pragma once
 
 #include "matrix.hpp"
 #include "geometry/vector.hpp"
@@ -20,8 +19,11 @@
 namespace modm
 {
 	// forward declaration
-	template <class T, uint8_t ROWS, uint8_t COLUMNS> class Matrix;
-	template <class T, uint8_t N> class Vector;
+	template <typename T, std::size_t ROWS, std::size_t COLUMNS>
+	class Matrix;
+
+	template <typename T, std::size_t N>
+	class Vector;
 
 	/**
 	 * \brief	Class for decomposing matrices
@@ -39,41 +41,41 @@ namespace modm
 	class LUDecomposition
 	{
 	public:
-		template <typename T, uint8_t N>
+		template <typename T, std::size_t SIZE>
 		static bool
-		decompose(const Matrix<T, N, N> &matrix,
-				Matrix<T, N, N> *l,
-				Matrix<T, N, N> *u);
+		decompose(const Matrix<T, SIZE, SIZE> &matrix,
+				Matrix<T, SIZE, SIZE> *l,
+				Matrix<T, SIZE, SIZE> *u);
 
-		template <typename T, uint8_t N>
+		template <typename T, std::size_t SIZE>
 		static bool
-		decompose(const Matrix<T, N, N> &matrix,
-				Matrix<T, N, N> *l,
-				Matrix<T, N, N> *u,
-				Matrix<T, N, N> *p);
+		decompose(const Matrix<T, SIZE, SIZE> &matrix,
+				Matrix<T, SIZE, SIZE> *l,
+				Matrix<T, SIZE, SIZE> *u,
+				Matrix<T, SIZE, SIZE> *p);
 
-		template <typename T, uint8_t N>
+		template <typename T, std::size_t SIZE>
 		static bool
-		decompose(const Matrix<T, N, N> &matrix,
-				Matrix<T, N, N> *l,
-				Matrix<T, N, N> *u,
-				Vector<int8_t, N> *p);
+		decompose(const Matrix<T, SIZE, SIZE> &matrix,
+				Matrix<T, SIZE, SIZE> *l,
+				Matrix<T, SIZE, SIZE> *u,
+				Vector<int8_t, SIZE> *p);
 
 
-		template <typename T, uint8_t N, uint8_t BXWIDTH>
+		template <typename T, std::size_t SIZE, std::size_t BXWIDTH>
 		static bool
-		solve(const Matrix<T, N, N> &l,
-				const Matrix<T, N, N> &u,
-				Matrix<T, N, BXWIDTH> *xb);
+		solve(const Matrix<T, SIZE, SIZE> &l,
+				const Matrix<T, SIZE, SIZE> &u,
+				Matrix<T, SIZE, BXWIDTH> *xb);
 
-		template <typename T, uint8_t N, uint8_t BXWIDTH>
+		template <typename T, std::size_t SIZE, std::size_t BXWIDTH>
 		static bool
-		solve(const Matrix<T, N, N> &A,
-				Matrix<T, N, BXWIDTH> *xb);
+		solve(const Matrix<T, SIZE, SIZE> &A,
+				Matrix<T, SIZE, BXWIDTH> *xb);
 
 
 	private:
-		template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH>
+		template<typename T, std::size_t OFFSET, std::size_t HEIGHT, std::size_t WIDTH>
 		class LUSubDecomposition
 		{
 		public:
@@ -89,19 +91,19 @@ namespace modm
 			static bool
 			decompose(T *u, T *l, int8_t *p);
 
-			template<uint8_t BXWIDTH>
+			template<std::size_t BXWIDTH>
 			static bool
 			solveLyEqualsB(
 					Matrix<T, HEIGHT, WIDTH> *l,
 					Matrix<T, HEIGHT, BXWIDTH> *bx);
 
-			template<uint8_t BXWIDTH>
+			template<std::size_t BXWIDTH>
 			static bool
 			solveUxEqualsY(
 					Matrix<T, HEIGHT, WIDTH> *u,
 					Matrix<T, HEIGHT, BXWIDTH> *bx);
 
-			template<uint8_t BXWIDTH>
+			template<std::size_t BXWIDTH>
 			static bool
 			solve(
 					const Matrix<T, HEIGHT, WIDTH> &l,
@@ -109,7 +111,7 @@ namespace modm
 					Matrix<T, HEIGHT, BXWIDTH> *bx);
 		};
 
-		template<typename T, uint8_t HEIGHT>
+		template<typename T, std::size_t HEIGHT>
 		class RowOperation
 		{
 		public:
@@ -145,7 +147,7 @@ namespace modm
 			swap(T *row1, T *row2);
 		};
 
-		template<typename T, uint8_t OFFSET, uint8_t WIDTH>
+		template<typename T, std::size_t OFFSET, std::size_t WIDTH>
 		class LUSubDecomposition<T, OFFSET, WIDTH, OFFSET>
 		{
 		public:
@@ -155,13 +157,13 @@ namespace modm
 			static bool
 			decomposeRecur(T *u, T *l);
 
-			template<uint8_t BXWIDTH>
+			template<std::size_t BXWIDTH>
 			static bool
 			solveUxEqualsY(
 					Matrix<T, WIDTH, OFFSET> *u,
 					Matrix<T, OFFSET, BXWIDTH> *bx);
 
-			template<uint8_t BXWIDTH>
+			template<std::size_t BXWIDTH>
 			static bool
 			solveLyEqualsB(
 					Matrix<T, WIDTH, OFFSET> *l,
@@ -170,5 +172,3 @@ namespace modm
 	};
 }
 #include "lu_decomposition_impl.hpp"
-
-#endif // MODM_LU_DECOMPOSITION_HPP
diff --git a/src/modm/math/lu_decomposition_impl.hpp b/src/modm/math/lu_decomposition_impl.hpp
index f79a9228ee..3068e1518b 100644
--- a/src/modm/math/lu_decomposition_impl.hpp
+++ b/src/modm/math/lu_decomposition_impl.hpp
@@ -9,14 +9,12 @@
  * License, v. 2.0. If a copy of the MPL was not distributed with this
  * file, You can obtain one at http://mozilla.org/MPL/2.0/.
  */
-// ----------------------------------------------------------------------------
+#pragma once
+#include "lu_decomposition.hpp"
 
-#ifndef MODM_LU_DECOMPOSITION_HPP
-	#error	"Don't include this file directly, use 'lu_decomposition.hpp' instead!"
-#endif
+#include <algorithm>
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t SIZE>
+template<typename T, std::size_t SIZE>
 bool
 modm::LUDecomposition::decompose(
 		const modm::Matrix<T, SIZE, SIZE> &matrix,
@@ -29,8 +27,7 @@ modm::LUDecomposition::decompose(
 	return LUSubDecomposition<T, 0, SIZE, SIZE>::decomposeRecur(u->ptr(), l->ptr());
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t SIZE>
+template<typename T, std::size_t SIZE>
 bool
 modm::LUDecomposition::decompose(
 		const modm::Matrix<T, SIZE, SIZE> &matrix,
@@ -38,16 +35,15 @@ modm::LUDecomposition::decompose(
 		modm::Matrix<T, SIZE, SIZE> *u,
 		modm::Vector<int8_t, SIZE> *p)
 {
-	for (uint_fast8_t i = 0; i < SIZE; ++i)	{
+	for (uint_fast8_t i = 0; i < SIZE; ++i)
 		(*p)[i] = i;
-	}
 	*u = matrix;
 	*l = modm::Matrix<T, SIZE, SIZE>::identityMatrix();
 
 	return LUSubDecomposition<T, 0, SIZE, SIZE>::decomposeRecur(u->ptr(), l->ptr(), p->ptr());
 }
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t SIZE>
+
+template<typename T, std::size_t SIZE>
 bool
 modm::LUDecomposition::decompose(
 		const modm::Matrix<T, SIZE, SIZE> &matrix,
@@ -56,16 +52,18 @@ modm::LUDecomposition::decompose(
 		modm::Matrix<T, SIZE, SIZE> *p)
 {
 	modm::Vector<int8_t, SIZE> pv;
-	if (not(decompose(matrix, l, u, &pv))){
-		return false;}
+
+	if (not(decompose(matrix, l, u, &pv)))
+		return false;
+
 	*p = modm::Matrix<T, SIZE, SIZE>::zeroMatrix();
-	for (int i=0; i<SIZE; i++){
-	(*p)[i][pv[i]] = 1;}
+	for (std::size_t i=0; i<SIZE; i++)
+		(*p)[i][pv[i]] = 1;
+
 	return true;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t SIZE, uint8_t BXWIDTH>
+template<typename T, std::size_t SIZE, std::size_t BXWIDTH>
 bool
 modm::LUDecomposition::solve(
 		const modm::Matrix<T, SIZE, SIZE> &l,
@@ -75,8 +73,7 @@ modm::LUDecomposition::solve(
 	return LUSubDecomposition<T, 0, SIZE, SIZE>::solve(l, u, xb);
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t SIZE, uint8_t BXWIDTH>
+template<typename T, std::size_t SIZE, std::size_t BXWIDTH>
 bool
 modm::LUDecomposition::solve(
 		const modm::Matrix<T, SIZE, SIZE> &A,
@@ -85,12 +82,14 @@ modm::LUDecomposition::solve(
 	modm::Matrix<T, SIZE, SIZE> l;
 	modm::Matrix<T, SIZE, SIZE> u;
 	modm::Matrix<T, SIZE, SIZE> p;
-	if(not(decompose(A, &l, &u, &p))){
+
+	if(not(decompose(A, &l, &u, &p)))
 		return false;
-	}
+
 	*xb = (p * (*xb));
-	if(not(solve(l, u, xb))){
-		return false;}
+	if(not(solve(l, u, xb)))
+		return false;
+
 	return true;
 }
 
@@ -98,71 +97,60 @@ modm::LUDecomposition::solve(
 // PRIVATE CLASS modm::LUDecomposition::RowOperation
 //=============================================================================
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t SIZE>
+template<typename T, std::size_t SIZE>
 void
 modm::LUDecomposition::RowOperation<T, SIZE>::multiply(T *intoRow, const T *row, const T &factor)
 {
-	for (uint_fast8_t i = 0; i < SIZE; ++i)	{
+	for (uint_fast8_t i = 0; i < SIZE; ++i)
 		intoRow[i] = row[i] * factor;
-	}
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t SIZE>
+template<typename T, std::size_t SIZE>
 void
 modm::LUDecomposition::RowOperation<T, SIZE>::addRowTimesFactor(T *intoRow, const T *srcRow, const T *addRow, const T &timesFactor)
 {
-	for (uint_fast8_t i = 0; i < SIZE; ++i)	{
+	for (uint_fast8_t i = 0; i < SIZE; ++i)
 		intoRow[i] = srcRow[i] + addRow[i] * timesFactor;
-	}
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t SIZE>
+template<typename T, std::size_t SIZE>
 void
 modm::LUDecomposition::RowOperation<T, SIZE>::swap(T *row1, T *row2)
 {
-	T tmp[SIZE];
+	// TODO prefer std::swap
 
-	memcpy(tmp, row1, SIZE*sizeof(T));
-	memcpy(row1, row2, SIZE*sizeof(T));
-	memcpy(row2, tmp, SIZE*sizeof(T));
+	T tmp[SIZE];
+	std::copy(row1, row1 + SIZE, tmp);
+	std::copy(row2, row2 + SIZE, row1);
+	std::copy(tmp, tmp + SIZE, row2);
 }
 
 //=============================================================================
 // PRIVATE CLASS modm::LUDecomposition::RowOperation<T, 0>
 //=============================================================================
 
-// ----------------------------------------------------------------------------
 template<typename T>
 void
 modm::LUDecomposition::RowOperation<T, 0>::multiply(
 		T * /* intoRow */, const T * /* row */, const T & /* factor */)
-{
-}
+{}
 
-// ----------------------------------------------------------------------------
 template<typename T>
 void
 modm::LUDecomposition::RowOperation<T, 0>::addRowTimesFactor(
 		T * /* intoRow */, const T * /* srcRow */, const T * /* addRow */, const T & /* timesFactor */)
-{
-}
+{}
 
-// ----------------------------------------------------------------------------
 template<typename T>
 void
 modm::LUDecomposition::RowOperation<T, 0>::swap(T * /* row1 */, T * /* row2 */)
-{
-}
+{}
 
 //=============================================================================
 // PRIVATE CLASS modm::LUDecomposition::LUSubDecomposition
 //=============================================================================
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH>
+template<typename T, std::size_t OFFSET, std::size_t HEIGHT, std::size_t WIDTH>
 bool
 modm::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::decomposeRecur(T * u, T * l)
 {
@@ -172,34 +160,29 @@ modm::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::decomposeRe
 	return LUSubDecomposition<T, OFFSET+1, HEIGHT, WIDTH>::decomposeRecur(u, l);
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH>
+template<typename T, std::size_t OFFSET, std::size_t HEIGHT, std::size_t WIDTH>
 bool
 modm::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::decompose(T * u, T * l)
 {
-	const uint8_t width = WIDTH;
-	const uint8_t height = HEIGHT;
-
 	// normalize the row
-	T factor = u[OFFSET*width + OFFSET];
-	l[OFFSET*width + OFFSET] = factor;
+	T factor = u[OFFSET*WIDTH + OFFSET];
+	l[OFFSET*WIDTH + OFFSET] = factor;
 
-	u[OFFSET*width + OFFSET] = 1;
-	RowOperation<T, WIDTH-OFFSET-1>::multiply(&u[OFFSET*width + OFFSET+1], &u[OFFSET*width + OFFSET+1], T(1.0)/factor);
+	u[OFFSET*WIDTH + OFFSET] = 1;
+	RowOperation<T, WIDTH-OFFSET-1>::multiply(&u[OFFSET*WIDTH + OFFSET+1], &u[OFFSET*WIDTH + OFFSET+1], T(1.0)/factor);
 
-	for (uint_fast8_t j = OFFSET+1; j < height; ++j)
+	for (uint_fast8_t j = OFFSET+1; j < HEIGHT; ++j)
 	{
-		factor = u[j*width + OFFSET];
-		l[j*width + OFFSET] = factor;
-		u[j*width + OFFSET] = 0;
-		RowOperation<T, WIDTH-OFFSET-1>::addRowTimesFactor(&u[j*width + OFFSET+1], &u[j*width + OFFSET+1], &u[OFFSET*width + OFFSET+1], -factor);
+		factor = u[j*WIDTH + OFFSET];
+		l[j*WIDTH + OFFSET] = factor;
+		u[j*WIDTH + OFFSET] = 0;
+		RowOperation<T, WIDTH-OFFSET-1>::addRowTimesFactor(&u[j*WIDTH + OFFSET+1], &u[j*WIDTH + OFFSET+1], &u[OFFSET*WIDTH + OFFSET+1], -factor);
 	}
 
 	return true;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH>
+template<typename T, std::size_t OFFSET, std::size_t HEIGHT, std::size_t WIDTH>
 bool
 modm::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::decomposeRecur(T *u, T *l, int8_t *p)
 {
@@ -209,21 +192,17 @@ modm::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::decomposeRe
 	return LUSubDecomposition<T, OFFSET+1, HEIGHT, WIDTH>::decomposeRecur(u, l, p);
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH>
+template<typename T, std::size_t OFFSET, std::size_t HEIGHT, std::size_t WIDTH>
 bool
 modm::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::decompose(T *u, T *l, int8_t *p)
 {
-	const uint8_t width = WIDTH;
-	const uint8_t height = HEIGHT;
-
 	//	swap with a lower row so that we have the highest value factor
 	// FIXME: we need to use something other than fabs here
-	T max = fabs(u[OFFSET*width + OFFSET]);
-	uint8_t maxRow = OFFSET;
-	for (uint_fast8_t j = OFFSET+1; j < height; ++j)
+	T max = fabs(u[OFFSET*WIDTH + OFFSET]);
+	std::size_t maxRow = OFFSET;
+	for (uint_fast8_t j = OFFSET+1; j < HEIGHT; ++j)
 	{
-		T v = fabs(u[j*width + OFFSET]);
+		T v = fabs(u[j*WIDTH + OFFSET]);
 		if (v > max)
 		{
 			max = v;
@@ -236,16 +215,15 @@ modm::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::decompose(T
 		uint16_t temp = p[OFFSET];
 		p[OFFSET] = p[maxRow];
 		p[maxRow] = temp;
-		RowOperation<T, WIDTH-OFFSET>::swap(&u[maxRow*width+OFFSET], &u[OFFSET*width+OFFSET]);
-		RowOperation<T, OFFSET>::swap(&l[maxRow*width], &l[OFFSET*width]);
+		RowOperation<T, WIDTH-OFFSET>::swap(&u[maxRow*WIDTH+OFFSET], &u[OFFSET*WIDTH+OFFSET]);
+		RowOperation<T, OFFSET>::swap(&l[maxRow*WIDTH], &l[OFFSET*WIDTH]);
 
 	}
 
 	return decompose(u, l);
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH> template<uint8_t BXWIDTH>
+template<typename T, std::size_t OFFSET, std::size_t HEIGHT, std::size_t WIDTH> template<std::size_t BXWIDTH>
 bool
 modm::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::solveLyEqualsB(
 		modm::Matrix<T, HEIGHT, WIDTH> *l,
@@ -255,9 +233,8 @@ modm::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::solveLyEqua
 	RowOperation<T, BXWIDTH>::multiply((*bx)[OFFSET], (*bx)[OFFSET], 1.0/factor);
 
 	// make sure there is a row under us
-	if (OFFSET >= HEIGHT-1)	{
+	if (OFFSET >= HEIGHT-1)
 		return true;
-	}
 
 	// substract the row so that all lower rows have a 0 at OFFSET
 	for (uint_fast8_t j = OFFSET+1; j < HEIGHT; ++j)
@@ -272,23 +249,21 @@ modm::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::solveLyEqua
 	return true;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH> template<uint8_t BXWIDTH>
+template<typename T, std::size_t OFFSET, std::size_t HEIGHT, std::size_t WIDTH> template<std::size_t BXWIDTH>
 bool
 modm::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::solveUxEqualsY(
 		modm::Matrix<T, HEIGHT, WIDTH> *u,
 		modm::Matrix<T, HEIGHT, BXWIDTH> *bx)
 {
 	// make sure there is a row under us
-	if (OFFSET >= HEIGHT-1)	{
+	if (OFFSET >= HEIGHT-1)
 		return true;
-	}
 
 	// solve the problem for SIZE-1
 	LUSubDecomposition<T, OFFSET+1, HEIGHT, WIDTH>::solveUxEqualsY(u, bx);
 
 	// substract the row so that all upper rows have a 0 at OFFSET
-	for (uint8_t j = 0; j < OFFSET+1; ++j)
+	for (std::size_t j = 0; j < OFFSET+1; ++j)
 	{
 		T factor = (*u)[j][OFFSET+1];
 		RowOperation<T, BXWIDTH>::addRowTimesFactor((*bx)[j], (*bx)[j], (*bx)[OFFSET+1], -factor);
@@ -298,8 +273,7 @@ modm::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::solveUxEqua
 	return true;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t OFFSET, uint8_t HEIGHT, uint8_t WIDTH> template<uint8_t BXWIDTH>
+template<typename T, std::size_t OFFSET, std::size_t HEIGHT, std::size_t WIDTH> template<std::size_t BXWIDTH>
 bool
 modm::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::solve(
 		const modm::Matrix<T, HEIGHT, WIDTH> &l,
@@ -316,36 +290,31 @@ modm::LUDecomposition::LUSubDecomposition<T, OFFSET, HEIGHT, WIDTH>::solve(
 	// start solving Ly = b
 
 	modm::Matrix<T, HEIGHT, WIDTH> lCopy(l);
-	if (!solveLyEqualsB(&lCopy, bx)) {
+	if (!solveLyEqualsB(&lCopy, bx))
 		return false;
-	}
 
 	modm::Matrix<T, HEIGHT, WIDTH> uCopy(u);
-	if (!solveUxEqualsY(&uCopy, bx)) {
+	if (!solveUxEqualsY(&uCopy, bx))
 		return false;
-	}
 
 	return true;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t OFFSET, uint8_t WIDTH>
+template<typename T, std::size_t OFFSET, std::size_t WIDTH>
 bool
 modm::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, OFFSET>::decomposeRecur(T * /* u */, T * /* l */, int8_t * /* p */)
 {
 	return true;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t OFFSET, uint8_t WIDTH>
+template<typename T, std::size_t OFFSET, std::size_t WIDTH>
 bool
 modm::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, OFFSET>::decomposeRecur(T * /* u */, T * /* l */)
 {
 	return true;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t OFFSET, uint8_t WIDTH> template<uint8_t BXWIDTH>
+template<typename T, std::size_t OFFSET, std::size_t WIDTH> template<std::size_t BXWIDTH>
 bool
 modm::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, OFFSET>::solveUxEqualsY(
 		modm::Matrix<T, WIDTH, OFFSET> * /* u */,
@@ -354,8 +323,7 @@ modm::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, OFFSET>::solveUxEqua
 	return true;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t OFFSET, uint8_t WIDTH> template<uint8_t BXWIDTH>
+template<typename T, std::size_t OFFSET, std::size_t WIDTH> template<std::size_t BXWIDTH>
 bool
 modm::LUDecomposition::LUSubDecomposition<T, OFFSET, WIDTH, OFFSET>::solveLyEqualsB(
 		modm::Matrix<T, WIDTH, OFFSET> * /* l */,
diff --git a/src/modm/math/matrix.hpp b/src/modm/math/matrix.hpp
index 1b2396773e..0d90f5bc81 100644
--- a/src/modm/math/matrix.hpp
+++ b/src/modm/math/matrix.hpp
@@ -11,11 +11,10 @@
  */
 // ----------------------------------------------------------------------------
 
-#ifndef MODM_MATRIX_HPP
-#define MODM_MATRIX_HPP
+#pragma once
 
 #include <cmath>
-#include <string.h>		// for memset() and memcmp()
+#include <algorithm>
 #include <stdint.h>
 
 #include <modm/io/iostream.hpp>
@@ -28,7 +27,7 @@ namespace modm
 	 * Having the width and height as template parameters has several
 	 * advantages over the tradition dynamic matrix class:
 	 *
-	 * - The compiler knows how many elements you have in your matrix and can
+	 * - The compiler knows how many ElementCount you have in your matrix and can
 	 *   unroll and optimize loops
 	 * - You can ensure that you are not doing operations on matrices with
 	 *   incompatible sizes (multiplication for example). The compiler will
@@ -48,33 +47,40 @@ namespace modm
 	 * \author	Niklas Hauser
 	 * \author	Fabian Greif
 	 */
-	template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+	template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 	class Matrix
 	{
 	public:
+		static constexpr std::size_t RowCount = ROWS;
+		static constexpr std::size_t ColumnCount = COLUMNS;
+		static constexpr std::size_t ElementCount = RowCount * ColumnCount;
+
+		T element[ElementCount];
+
 		/**
 		 * \brief	Default Constructor
 		 *
-		 * Creates a Matrix with uninitialized elements. Use zeroMatrix() to
-		 * create a matrix with all elements set to zero.
+		 * Creates a Matrix with uninitialized ElementCount. Use zeroMatrix() to
+		 * create a matrix with all ElementCount set to zero.
 		 */
-		Matrix();
+		constexpr Matrix() = default;
 
 		/**
 		 * \brief	Create a matrix from an array
 		 *
 		 * Example:
 		 * \code
-		 * const int16_t m[6] = {
+		 *
+		 * modm::Matrix<int16_t, 3, 2> a({
 		 *     1, 2,
 		 *     3, 4,
 		 *     5, 6,
-		 * };
-		 *
-		 * modm::Matrix<int16_t, 3, 2> a(m);
+		 * });
 		 * \endcode
 		 */
-		Matrix(const T *data);
+		constexpr Matrix(const T *data) {
+			std::copy(data, data + ElementCount, element);
+		}
 
 		/**
 		 * \brief	Get a zero matrix
@@ -98,52 +104,49 @@ namespace modm
 		 * \brief	Create a new sub matrix
 		 *
 		 */
-		template <uint8_t MR, uint8_t MC>
+		template <std::size_t MR, std::size_t MC>
 		Matrix<T, MR, MC>
-		subMatrix(uint8_t row, uint8_t column) const;
+		subMatrix(std::size_t row, std::size_t column) const;
 
 		bool operator == (const Matrix &m) const;
 		bool operator != (const Matrix &m) const;
 
-		const T*
-		operator [] (uint8_t row) const;
-
-		T*
-		operator [] (uint8_t row);
+		T* operator [] (std::size_t row);
+		const T* operator [] (std::size_t row) const;
 
-		inline uint8_t
-		getNumberOfRows() const;
+		std::size_t getNumberOfRows() const
+		{ return RowCount; }
 
-		inline uint8_t
-		getNumberOfColumns() const;
+		std::size_t getNumberOfColumns() const
+		{ return ColumnCount; }
 
 		Matrix<T, 1, COLUMNS>
-		getRow(uint8_t index) const;
+		getRow(std::size_t index) const;
 
 		Matrix<T, ROWS, 1>
-		getColumn(uint8_t index) const;
+		getColumn(std::size_t index) const;
 
-		// TODO remove these?
-		const T* ptr() const;
-		T* ptr();
+		// DEPRECATED element is public
+		T* ptr() { return element; }
+		const T* ptr() const { return element;}
 
-		Matrix operator - ();
-		Matrix operator + (const Matrix &rhs) const;
-		Matrix operator - (const Matrix &rhs) const;
-		Matrix operator * (const T &rhs) const;		///< Scalar multiplication
-		Matrix operator / (const T &rhs) const;		///< Scalar division
+		constexpr Matrix operator - ();
+		constexpr Matrix operator + (const Matrix &rhs) const;
+		constexpr Matrix operator - (const Matrix &rhs) const;
+		constexpr Matrix operator * (T rhs) const;		///< Scalar multiplication
+		constexpr Matrix operator / (T rhs) const;		///< Scalar division
 
 		Matrix& operator += (const Matrix &rhs);
 		Matrix& operator -= (const Matrix &rhs);
-		Matrix& operator *= (const T &rhs);			///< Scalar multiplication
-		Matrix& operator /= (const T &rhs);			///< Scalar division
+		Matrix& operator *= (T rhs);			///< Scalar multiplication
+		Matrix& operator /= (T rhs);			///< Scalar division
 
 		/// Matrix multiplication with matrices with the same size
 		Matrix operator *= (const Matrix &rhs);
 
 		/// Matrix multiplication with different size matrices
-		template<uint8_t RHSCOL>
-		Matrix<T, ROWS, RHSCOL>
+		template<std::size_t RHSCOL>
+		constexpr Matrix<T, ROWS, RHSCOL>
 		operator * (const Matrix<T, COLUMNS, RHSCOL> &rhs) const;
 
 		Matrix<T, COLUMNS, ROWS>
@@ -154,7 +157,7 @@ namespace modm
 		 *
 		 * \warning	Will only work if the matrix is square!
 		 */
-		inline void
+		void
 		transpose();
 
 		/**
@@ -162,7 +165,7 @@ namespace modm
 		 *
 		 * Uses modm::determinant(*this);
 		 */
-		inline T
+		T
 		determinant() const;
 
 		// TODO Implement these
@@ -181,62 +184,52 @@ namespace modm
 		replace(const U *data);
 
 		///
-		template<uint8_t MW, uint8_t MH>
+		template<std::size_t MW, std::size_t MH>
 		Matrix&
-		replace(uint8_t row, uint8_t column, const Matrix<T, MW, MH> &m);
+		replace(std::size_t row, std::size_t column, const Matrix<T, MW, MH> &m);
 
 		Matrix&
-		replaceRow(uint8_t index, const Matrix<T, 1, COLUMNS> &m);
+		replaceRow(std::size_t index, const Matrix<T, 1, COLUMNS> &m);
 
 		Matrix&
-		replaceColumn(uint8_t index, const Matrix<T, ROWS, 1> &m);
+		replaceColumn(std::size_t index, const Matrix<T, ROWS, 1> &m);
 
 
 		Matrix<T, ROWS, COLUMNS+1>
-		addColumn(uint8_t index, const Matrix<T, ROWS, 1> &c) const;
+		addColumn(std::size_t index, const Matrix<T, ROWS, 1> &c) const;
 
 		Matrix<T, ROWS+1, COLUMNS>
-		addRow(uint8_t index, const Matrix<T, 1, COLUMNS> &r) const;
+		addRow(std::size_t index, const Matrix<T, 1, COLUMNS> &r) const;
 
 
 		Matrix<T, ROWS, COLUMNS-1>
-		removeColumn(uint8_t index) const;
+		removeColumn(std::size_t index) const;
 
 		Matrix<T, ROWS-1, COLUMNS>
-		removeRow(uint8_t index) const;
-
-	public:
-		T element[ROWS * COLUMNS];
+		removeRow(std::size_t index) const;
 
 	private:
-		/// Size of the Matrix in Bytes
-		inline size_t
-		getSize() const;
-
-		/// Number of elements in the Matrix (rows * columns)
-		inline uint8_t
-		getNumberOfElements() const;
+		std::size_t getSize() const
+		{ return ElementCount * sizeof(T); }
 	};
 
-	template<typename T, uint8_t WIDTH, uint8_t HEIGHT>
+	template<typename T, std::size_t WIDTH, std::size_t HEIGHT>
 	IOStream&
 	operator << (IOStream&, const Matrix<T, WIDTH, HEIGHT>&);
 
-	typedef Matrix<float, 1, 1> Matrix1f;
-	typedef Matrix<float, 2, 2> Matrix2f;
-	typedef Matrix<float, 3, 3> Matrix3f;
-	typedef Matrix<float, 4, 4> Matrix4f;
+	using Matrix1f = Matrix<float, 1, 1>;
+	using Matrix2f = Matrix<float, 2, 2>;
+	using Matrix3f = Matrix<float, 3, 3>;
+	using Matrix4f = Matrix<float, 4, 4>;
 
 	// ------------------------------------------------------------------------
 	/// \internal
 	template<typename T>
-	T
-	determinant(const modm::Matrix<T, 1, 1> &m);
+	T determinant(const modm::Matrix<T, 1, 1> &m);
 
 	/// \internal
 	template<typename T>
-	T
-	determinant(const modm::Matrix<T, 2, 2> &m);
+	T determinant(const modm::Matrix<T, 2, 2> &m);
 
 	/**
 	 * \brief	Calculate the determinant
@@ -244,11 +237,8 @@ namespace modm
 	 * \param	m	Matrix
 	 * \ingroup	modm_math_matrix
 	 */
-	template<typename T, uint8_t N>
-	T
-	determinant(const modm::Matrix<T, N, N> &m);
+	template<typename T, std::size_t N>
+	T determinant(const modm::Matrix<T, N, N> &m);
 }
 
-#include "matrix_impl.hpp"
-
-#endif	// MODM_MATRIX_HPP
+#include "matrix_impl.hpp"
\ No newline at end of file
diff --git a/src/modm/math/matrix_impl.hpp b/src/modm/math/matrix_impl.hpp
index ebb25d4af3..afabda163e 100644
--- a/src/modm/math/matrix_impl.hpp
+++ b/src/modm/math/matrix_impl.hpp
@@ -9,29 +9,12 @@
  * License, v. 2.0. If a copy of the MPL was not distributed with this
  * file, You can obtain one at http://mozilla.org/MPL/2.0/.
  */
-// ----------------------------------------------------------------------------
-
-#ifndef MODM_MATRIX_HPP
-#	error	"Don't include this file directly, use 'matrix.hpp' instead!"
-#endif
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>::Matrix()
-{
-}
+#pragma once
+#include "matrix.hpp"
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>::Matrix(const T *data)
-{
-	for (uint_fast8_t i = 0; i < getNumberOfElements(); ++i) {
-		element[i] = data[i];
-	}
-}
+#include <algorithm>
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 const modm::Matrix<T, ROWS, COLUMNS>&
 modm::Matrix<T, ROWS, COLUMNS>::identityMatrix()
 {
@@ -41,17 +24,11 @@ modm::Matrix<T, ROWS, COLUMNS>::identityMatrix()
 	if (!hasIdentityMatrix)
 	{
 		if (ROWS < COLUMNS)
-		{
-			for (uint_fast8_t i = 0; i < ROWS; ++i) {
+			for (std::size_t i = 0; i < ROWS; ++i)
 				matrix[i][i] = 1;
-			}
-		}
 		else
-		{
-			for (uint_fast8_t i = 0; i < COLUMNS; ++i) {
+			for (std::size_t i = 0; i < COLUMNS; ++i)
 				matrix[i][i] = 1;
-			}
-		}
 
 		hasIdentityMatrix = true;
 	}
@@ -60,7 +37,7 @@ modm::Matrix<T, ROWS, COLUMNS>::identityMatrix()
 }
 
 // ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 const modm::Matrix<T, ROWS, COLUMNS>&
 modm::Matrix<T, ROWS, COLUMNS>::zeroMatrix()
 {
@@ -69,7 +46,7 @@ modm::Matrix<T, ROWS, COLUMNS>::zeroMatrix()
 
 	if (!hasZeroMatrix)
 	{
-		memset(matrix.ptr(), 0, matrix.getSize());
+		std::fill(matrix.element, matrix.element + ElementCount, 0);
 		hasZeroMatrix = true;
 	}
 
@@ -77,252 +54,196 @@ modm::Matrix<T, ROWS, COLUMNS>::zeroMatrix()
 }
 
 // ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 bool
 modm::Matrix<T, ROWS, COLUMNS>::operator == (const modm::Matrix<T, ROWS, COLUMNS> &m) const
 {
-	return memcmp(element, m.element, getSize()) == 0;
+	return std::equal(element, element + ElementCount, m.element);
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 bool
 modm::Matrix<T, ROWS, COLUMNS>::operator != (const modm::Matrix<T, ROWS, COLUMNS> &m) const
 {
-	return memcmp(element, m.element, getSize()) != 0;
+	return !std::equal(element, element + ElementCount, m.element);
 }
 
 // ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 modm::Matrix<T, 1, COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>::getRow(uint8_t index) const
+modm::Matrix<T, ROWS, COLUMNS>::getRow(std::size_t index) const
 {
 	return subMatrix<1, COLUMNS>(index, 0);
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 modm::Matrix<T, ROWS, 1>
-modm::Matrix<T, ROWS, COLUMNS>::getColumn(uint8_t index) const
+modm::Matrix<T, ROWS, COLUMNS>::getColumn(std::size_t index) const
 {
 	return subMatrix<ROWS, 1>(0, index);
 }
 
 // ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 T*
-modm::Matrix<T, ROWS, COLUMNS>::operator [] (uint8_t row)
+modm::Matrix<T, ROWS, COLUMNS>::operator [] (std::size_t row)
 {
 	return &element[row * COLUMNS];
 }
 
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 const T*
-modm::Matrix<T, ROWS, COLUMNS>::operator [] (uint8_t row) const
+modm::Matrix<T, ROWS, COLUMNS>::operator [] (std::size_t row) const
 {
 	return &element[row * COLUMNS];
 }
 
 // ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-uint8_t
-modm::Matrix<T, ROWS, COLUMNS>::getNumberOfRows() const
-{
-	return ROWS;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-uint8_t
-modm::Matrix<T, ROWS, COLUMNS>::getNumberOfColumns() const
-{
-	return COLUMNS;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-const T*
-modm::Matrix<T, ROWS, COLUMNS>::ptr() const
-{
-	return element;
-}
-
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-T*
-modm::Matrix<T, ROWS, COLUMNS>::ptr()
-{
-	return element;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
+constexpr modm::Matrix<T, ROWS, COLUMNS>
 modm::Matrix<T, ROWS, COLUMNS>::operator - ()
 {
 	modm::Matrix<T, ROWS, COLUMNS> m;
-	for (uint_fast8_t i = 0; i < getNumberOfElements(); ++i) {
-		m.element[i] = -this->element[i];
-	}
+	for (std::size_t i = 0; i < ElementCount; ++i)
+		m.element[i] = -element[i];
 
 	return m;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
+constexpr modm::Matrix<T, ROWS, COLUMNS>
 modm::Matrix<T, ROWS, COLUMNS>::operator - (const modm::Matrix<T, ROWS, COLUMNS> &rhs) const
 {
 	modm::Matrix<T, ROWS, COLUMNS> m;
 
-	for (uint_fast8_t i = 0; i < getNumberOfElements(); ++i) {
+	for (std::size_t i = 0; i < ElementCount; ++i)
 		m.element[i] = element[i] - rhs.element[i];
-	}
 
 	return m;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
+constexpr modm::Matrix<T, ROWS, COLUMNS>
 modm::Matrix<T, ROWS, COLUMNS>::operator + (const modm::Matrix<T, ROWS, COLUMNS> &rhs) const
 {
 	modm::Matrix<T, ROWS, COLUMNS> m;
 
-	for (uint_fast8_t i = 0; i < getNumberOfElements(); ++i) {
+	for (std::size_t i = 0; i < ElementCount; ++i)
 		m.element[i] = element[i] + rhs.element[i];
-	}
 
 	return m;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>&
-modm::Matrix<T, ROWS, COLUMNS>::operator += (const modm::Matrix<T, ROWS, COLUMNS> &rhs)
-{
-	for (uint_fast8_t i = 0; i < getNumberOfElements(); ++i) {
-		element[i] += rhs.element[i];
-	}
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>&
-modm::Matrix<T, ROWS, COLUMNS>::operator -= (const modm::Matrix<T, ROWS, COLUMNS> &rhs)
-{
-	for (uint_fast8_t i = 0; i < getNumberOfElements(); ++i) {
-		element[i] -= rhs.element[i];
-	}
-
-	return *this;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-template<uint8_t RHSCOL>
-modm::Matrix<T, ROWS, RHSCOL>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
+template<std::size_t RHSCOL>
+constexpr modm::Matrix<T, ROWS, RHSCOL>
 modm::Matrix<T, ROWS, COLUMNS>::operator * (const Matrix<T, COLUMNS, RHSCOL> &rhs) const
 {
 	modm::Matrix<T, ROWS, RHSCOL> m;
 
-	for (uint_fast8_t i = 0; i < ROWS; ++i)
+	for (std::size_t i = 0; i < ROWS; ++i)
 	{
-		for (uint_fast8_t j = 0; j < RHSCOL; ++j)
+		for (std::size_t j = 0; j < RHSCOL; ++j)
 		{
 			m[i][j] = element[i * COLUMNS] * rhs[0][j];
-			for (uint_fast8_t x = 1; x < COLUMNS; ++x)
-			{
+			for (std::size_t x = 1; x < COLUMNS; ++x)
 				m[i][j] += element[i * COLUMNS + x] * rhs[x][j];
-			}
 		}
 	}
 	return m;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>::operator *= (const modm::Matrix<T, ROWS, COLUMNS> &rhs)
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
+constexpr modm::Matrix<T, ROWS, COLUMNS>
+modm::Matrix<T, ROWS, COLUMNS>::operator * (T rhs) const
 {
-	(*this) = (*this) * rhs;
-	return *this;
+	modm::Matrix<T, ROWS, COLUMNS> m;
+	for (std::size_t i = 0; i < ElementCount; ++i)
+		m.element[i] = element[i] * rhs;
+
+	return m;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>::operator * (const T &rhs) const
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
+constexpr modm::Matrix<T, ROWS, COLUMNS>
+modm::Matrix<T, ROWS, COLUMNS>::operator / (T rhs) const
 {
 	modm::Matrix<T, ROWS, COLUMNS> m;
-	for (uint_fast8_t i = 0; i < getNumberOfElements(); ++i) {
-		m.element[i] = element[i] * rhs;
-	}
+
+	float oneOverRhs = 1.0f / rhs;
+
+	for (std::size_t i = 0; i < ElementCount; ++i)
+		m.element[i] = element[i] * oneOverRhs;
 
 	return m;
 }
 
 // ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 modm::Matrix<T, ROWS, COLUMNS>&
-modm::Matrix<T, ROWS, COLUMNS>::operator *= (const T &rhs)
+modm::Matrix<T, ROWS, COLUMNS>::operator += (const modm::Matrix<T, ROWS, COLUMNS> &rhs)
 {
-	for (uint_fast8_t i = 0; i < getNumberOfElements(); ++i) {
-		element[i] *= rhs;
-	}
+	for (std::size_t i = 0; i < ElementCount; ++i)
+		element[i] += rhs.element[i];
 
 	return *this;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>::operator / (const T &rhs) const
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
+modm::Matrix<T, ROWS, COLUMNS>&
+modm::Matrix<T, ROWS, COLUMNS>::operator -= (const modm::Matrix<T, ROWS, COLUMNS> &rhs)
 {
-	modm::Matrix<T, ROWS, COLUMNS> m;
+	for (std::size_t i = 0; i < ElementCount; ++i)
+		element[i] -= rhs.element[i];
 
-	float oneOverRhs = 1.0f / rhs;
+	return *this;
+}
 
-	for (uint_fast8_t i = 0; i < getNumberOfElements(); ++i) {
-		m.element[i] = element[i] * oneOverRhs;
-	}
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
+modm::Matrix<T, ROWS, COLUMNS>
+modm::Matrix<T, ROWS, COLUMNS>::operator *= (const modm::Matrix<T, ROWS, COLUMNS> &rhs)
+{
+	(*this) = (*this) * rhs;
+	return *this;
+}
 
-	return m;
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
+modm::Matrix<T, ROWS, COLUMNS>&
+modm::Matrix<T, ROWS, COLUMNS>::operator *= (T rhs)
+{
+	for (std::size_t i = 0; i < ElementCount; ++i)
+		element[i] *= rhs;
+
+	return *this;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 modm::Matrix<T, ROWS, COLUMNS>&
-modm::Matrix<T, ROWS, COLUMNS>::operator /= (const T &rhs)
+modm::Matrix<T, ROWS, COLUMNS>::operator /= (T rhs)
 {
 	float oneOverRhs = 1.0f / rhs;
 
-	for (uint_fast8_t i = 0; i < getNumberOfElements(); ++i) {
+	for (std::size_t i = 0; i < ElementCount; ++i)
 		element[i] *= oneOverRhs;
-	}
 
 	return *this;
 }
 
 // ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 modm::Matrix<T, COLUMNS, ROWS>
 modm::Matrix<T, ROWS, COLUMNS>::asTransposed() const
 {
 	modm::Matrix<T, COLUMNS, ROWS> m;
 
-	for (uint_fast8_t i = 0; i < ROWS; ++i) {
-		for (uint_fast8_t j = 0; j < COLUMNS; ++j) {
+	for (std::size_t i = 0; i < ROWS; ++i)
+		for (std::size_t j = 0; j < COLUMNS; ++j)
 			m.element[j * ROWS + i] = element[i * COLUMNS + j];
-		}
-	}
 
 	return m;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 void
 modm::Matrix<T, ROWS, COLUMNS>::transpose()
 {
@@ -332,7 +253,7 @@ modm::Matrix<T, ROWS, COLUMNS>::transpose()
 }
 
 // ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 inline T
 modm::Matrix<T, ROWS, COLUMNS>::determinant() const
 {
@@ -342,43 +263,37 @@ modm::Matrix<T, ROWS, COLUMNS>::determinant() const
 }
 
 // ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 bool
 modm::Matrix<T, ROWS, COLUMNS>::hasNan() const
 {
-	for (uint_fast8_t i = 0; i < getNumberOfElements(); ++i) {
-		if (isnan(element[i])) {
+	for (std::size_t i = 0; i < ElementCount; ++i)
+		if (isnan(element[i]))
 			return true;
-		}
-	}
 
 	return false;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 bool
 modm::Matrix<T, ROWS, COLUMNS>::hasInf() const
 {
-	for (uint_fast8_t i = 0; i < getNumberOfElements(); ++i) {
-		if (isinf(element[i])) {
+	for (std::size_t i = 0; i < ElementCount; ++i)
+		if (isinf(element[i]))
 			return true;
-		}
-	}
 
 	return false;
 }
 
 // ----------------------------------------------------------------------------
-template<typename T, uint8_t WIDTH, uint8_t HEIGHT>
+template<typename T, std::size_t WIDTH, std::size_t HEIGHT>
 modm::Matrix<T, WIDTH, HEIGHT>
 operator * (int8_t lhs, const modm::Matrix<T, WIDTH, HEIGHT> &m)
 {
 	return m * lhs;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t WIDTH, uint8_t HEIGHT>
+template<typename T, std::size_t WIDTH, std::size_t HEIGHT>
 modm::Matrix<T, WIDTH, HEIGHT>
 operator * (float lhs, const modm::Matrix<T, WIDTH, HEIGHT> &m)
 {
@@ -386,15 +301,14 @@ operator * (float lhs, const modm::Matrix<T, WIDTH, HEIGHT> &m)
 }
 
 // ----------------------------------------------------------------------------
-/*template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+/*template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 void
 modm::Matrix<T, ROWS, COLUMNS>::inverse()
 {
 	*this = inversed();
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 modm::Matrix<T, ROWS, COLUMNS>
 modm::Matrix<T, ROWS, COLUMNS>::inversed() const
 {
@@ -404,210 +318,169 @@ modm::Matrix<T, ROWS, COLUMNS>::inversed() const
 }*/
 
 // ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-size_t
-modm::Matrix<T, ROWS, COLUMNS>::getSize() const
-{
-	return getNumberOfElements() * sizeof(T);
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-uint8_t
-modm::Matrix<T, ROWS, COLUMNS>::getNumberOfElements() const
-{
-	return ROWS * COLUMNS;
-}
-
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-template <uint8_t MR, uint8_t MC>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
+template <std::size_t MR, std::size_t MC>
 modm::Matrix<T, MR, MC>
-modm::Matrix<T, ROWS, COLUMNS>::subMatrix(uint8_t row, uint8_t column) const
+modm::Matrix<T, ROWS, COLUMNS>::subMatrix(std::size_t row, std::size_t column) const
 {
 	static_assert(MR <= ROWS, "sub matrix must be smaller than the original");
 	static_assert(MC <= COLUMNS, "sub matrix must be smaller than the original");
 
 	Matrix<T, MR, MC> sub;
-	for (uint_fast8_t i = 0; i < MR; ++i) {
-		for (uint_fast8_t j = 0; j < MC; ++j) {
+	for (std::size_t i = 0; i < MR; ++i)
+		for (std::size_t j = 0; j < MC; ++j)
 			sub[i][j] = (*this)[i + row][j + column];
-		}
-	}
 
 	return sub;
 }
 
 // ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS> template<typename U>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS> template<typename U>
 modm::Matrix<T, ROWS, COLUMNS>&
 modm::Matrix<T, ROWS, COLUMNS>::replace(const U *data)
 {
-	for (uint_fast8_t i = 0; i < getNumberOfElements(); ++i) {
+	for (std::size_t i = 0; i < ElementCount; ++i)
 		element[i] = data[i];
-	}
 
 	return *this;
 }
 
 // ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
-template <uint8_t MR, uint8_t MC>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
+template <std::size_t MR, std::size_t MC>
 modm::Matrix<T, ROWS, COLUMNS>&
-modm::Matrix<T, ROWS, COLUMNS>::replace(uint8_t row, uint8_t column, const modm::Matrix<T, MR, MC> &m)
+modm::Matrix<T, ROWS, COLUMNS>::replace(std::size_t row, std::size_t column, const modm::Matrix<T, MR, MC> &m)
 {
 	static_assert(MR <= ROWS, "replacement matrix can't be larger than the original");
 	static_assert(MC <= COLUMNS, "replacement matrix can't be larger than the original");
 
-	for (uint_fast8_t i = 0; i < MR && (i + row) < ROWS; ++i)
-	{
-		for (uint_fast8_t j = 0; j < MC && (j + column) < COLUMNS; ++j)
-		{
+	for (std::size_t i = 0; i < MR && (i + row) < ROWS; ++i)
+		for (std::size_t j = 0; j < MC && (j + column) < COLUMNS; ++j)
 			element[(i + row) * COLUMNS + (j + column)] = m[i][j];
-		}
-	}
 
 	return *this;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 modm::Matrix<T, ROWS, COLUMNS>&
-modm::Matrix<T, ROWS, COLUMNS>::replaceRow(uint8_t index, const modm::Matrix<T, 1, COLUMNS> &m)
+modm::Matrix<T, ROWS, COLUMNS>::replaceRow(std::size_t index, const modm::Matrix<T, 1, COLUMNS> &m)
 {
 	return replace(index, 0, m);
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 modm::Matrix<T, ROWS, COLUMNS>&
-modm::Matrix<T, ROWS, COLUMNS>::replaceColumn(uint8_t index, const modm::Matrix<T, ROWS, 1> &m)
+modm::Matrix<T, ROWS, COLUMNS>::replaceColumn(std::size_t index, const modm::Matrix<T, ROWS, 1> &m)
 {
 	return replace(0, index, m);
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 modm::Matrix<T, ROWS+1, COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>::addRow(uint8_t index, const modm::Matrix<T, 1, COLUMNS> &r) const
+modm::Matrix<T, ROWS, COLUMNS>::addRow(std::size_t index, const modm::Matrix<T, 1, COLUMNS> &r) const
 {
 	modm::Matrix<T, ROWS+1, COLUMNS> m;
-	uint_fast8_t i = 0, ri = 0;
+	std::size_t i = 0, ri = 0;
 
-	for (; i < index; ++i) {
+	for (; i < index; ++i)
 		m.replaceRow(ri++, getRow(i));
-	}
+
 	m.replaceRow(ri++, r);
-	for (; i < ROWS+1; ++i) {
+
+	for (; i < ROWS+1; ++i)
 		m.replaceRow(ri++, getRow(i));
-	}
 
 	return m;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 modm::Matrix<T, ROWS, COLUMNS+1>
-modm::Matrix<T, ROWS, COLUMNS>::addColumn(uint8_t index, const modm::Matrix<T, ROWS, 1> &c) const
+modm::Matrix<T, ROWS, COLUMNS>::addColumn(std::size_t index, const modm::Matrix<T, ROWS, 1> &c) const
 {
 	modm::Matrix<T, ROWS, COLUMNS+1> m;
-	uint_fast8_t i = 0, ci = 0;
+	std::size_t i = 0, ci = 0;
 
-	for (; i < index; ++i) {
+	for (; i < index; ++i)
 		m.replaceColumn(ci++, getColumn(i));
-	}
+
 	m.replaceColumn(ci++, c);
-	for (; i < COLUMNS+1; ++i) {
+
+	for (; i < COLUMNS+1; ++i)
 		m.replaceColumn(ci++, getColumn(i));
-	}
 
 	return m;
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 modm::Matrix<T, ROWS-1, COLUMNS>
-modm::Matrix<T, ROWS, COLUMNS>::removeRow(uint8_t index ) const
+modm::Matrix<T, ROWS, COLUMNS>::removeRow(std::size_t index ) const
 {
 	if (index == 0)
-	{
 		return subMatrix<ROWS-1, COLUMNS>(1, 0);
-	}
 	else if (index == (ROWS - 1))
-	{
 		return subMatrix<ROWS-1, COLUMNS>(0, 0);
-	}
 	else
 	{
 		Matrix<T, ROWS-1, COLUMNS> m;
-		uint_fast8_t i = 0, ri = 0;
+		std::size_t i = 0, ri = 0;
 
-		for (; i < index; ++i) {
+		for (; i < index; ++i)
 			m.replaceRow(ri++, getRow(i));
-		}
+
 		++i; // skip one row
-		for (; i < ROWS; ++i) {
+
+		for (; i < ROWS; ++i)
 			m.replaceRow(ri++, getRow(i));
-		}
 
 		return m;
 	}
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 modm::Matrix<T, ROWS, COLUMNS-1>
-modm::Matrix<T, ROWS, COLUMNS>::removeColumn(uint8_t index) const
+modm::Matrix<T, ROWS, COLUMNS>::removeColumn(std::size_t index) const
 {
 	if (index == 0)
-	{
 		return subMatrix<ROWS, COLUMNS-1>(0, 1);
-	}
 	else if (index == (COLUMNS - 1))
-	{
 		return subMatrix<ROWS, COLUMNS-1>(0, 0);
-	}
 	else
 	{
 		Matrix<T, ROWS, COLUMNS-1> m;
-		uint_fast8_t i = 0, ci = 0;
+		std::size_t i = 0, ci = 0;
 
-		for (; i < index; ++i) {
+		for (; i < index; ++i)
 			m.replaceColumn(ci++, getColumn(i));
-		}
+
 		++i; // skip one column
-		for (; i < COLUMNS; ++i) {
+
+		for (; i < COLUMNS; ++i)
 			m.replaceColumn(ci++, getColumn(i));
-		}
 
 		return m;
 	}
 }
 
 // ----------------------------------------------------------------------------
-template<typename T, uint8_t ROWS, uint8_t COLUMNS>
+template<typename T, std::size_t ROWS, std::size_t COLUMNS>
 modm::IOStream&
 modm::operator << (modm::IOStream& os, const modm::Matrix<T, ROWS, COLUMNS> &m)
 {
 	os << "{ ";
 
-	for (uint_fast8_t i = 0; i < ROWS; ++i)
+	for (std::size_t i = 0; i < ROWS; ++i)
 	{
 		os << "{ ";
-		for (uint_fast8_t j = 0; j < COLUMNS; ++j)
+		for (std::size_t j = 0; j < COLUMNS; ++j)
 		{
 			os << m.element[i * COLUMNS + j];
 			if (j < COLUMNS-1)
-			{
 				os << ", ";
-			}
 		}
 		os << " }";
 
 		if (i < ROWS-1)
-		{
 			os << ", \n";
-		}
 	}
 	os << " }";
 	return os;
@@ -621,7 +494,6 @@ modm::determinant(const modm::Matrix<T, 1, 1> &m)
 	return m[0][0];
 }
 
-// ----------------------------------------------------------------------------
 template<typename T>
 T
 modm::determinant(const modm::Matrix<T, 2, 2> &m)
@@ -629,35 +501,29 @@ modm::determinant(const modm::Matrix<T, 2, 2> &m)
 	return (m[0][0] * m[1][1] - m[0][1] * m[1][0]);
 }
 
-// ----------------------------------------------------------------------------
-template<typename T, uint8_t N>
+template<typename T, std::size_t N>
 T
 modm::determinant(const modm::Matrix<T, N, N> &m)
 {
 	// not the most efficient way, but should work for now...
 	T value = 0;
 	int8_t factor = 1;
-	for (uint_fast8_t i = 0; i < N; ++i)
+	for (std::size_t i = 0; i < N; ++i)
 	{
 		T coeff = m[0][i];
 		modm::Matrix<T, N-1, N-1> subM;
 
-		for (uint_fast8_t x = 0; x < i; ++x) {
-			for (uint_fast8_t y = 1; y < N; ++y) {
+		for (std::size_t x = 0; x < i; ++x)
+			for (std::size_t y = 1; y < N; ++y)
 				subM[y-1][x] = m[y][x];
-			}
-		}
 
-		for (uint_fast8_t x = i+1; x < N; ++x) {
-			for (uint_fast8_t y = 1; y < N; ++y) {
+		for (std::size_t x = i+1; x < N; ++x)
+			for (std::size_t y = 1; y < N; ++y)
 				subM[y-1][x-1] = m[y][x];
-			}
-		}
 
 		value += coeff * factor * determinant(subM);
 		factor *= -1;
 	}
 
 	return value;
-}
-
+}
\ No newline at end of file
diff --git a/test/config/hosted.xml b/test/config/hosted.xml
index 660ea5c22f..ff96334d73 100644
--- a/test/config/hosted.xml
+++ b/test/config/hosted.xml
@@ -7,6 +7,6 @@
   <modules>
     <module>modm:platform:core</module>
     <module>modm:driver:terminal</module>
-    <module>modm-test:test:**</module>
+    <module>modm-test:test:math</module>
   </modules>
 </library>
diff --git a/test/modm/math/geometry/location_2d_test.cpp b/test/modm/math/geometry/location_2d_test.cpp
index b40322855f..e32ebc4e8f 100644
--- a/test/modm/math/geometry/location_2d_test.cpp
+++ b/test/modm/math/geometry/location_2d_test.cpp
@@ -45,7 +45,7 @@ Location2DTest::testAccessors()
 
 	TEST_ASSERT_EQUALS(location.getPosition(), modm::Vector2i(30, 40));
 
-	location.setPosition(50, 60);
+	location.setPosition({50, 60});
 
 	TEST_ASSERT_EQUALS(location.getPosition(), modm::Vector2i(50, 60));
 
@@ -110,11 +110,9 @@ Location2DTest::testMove()
 void
 Location2DTest::testConvert()
 {
-	modm::Location2D<float> a(
-			modm::Vector<float, 2>(-10.65, 20.31),
-			M_PI);
+	modm::Location2D<float> a({-10.65, 20.31}, M_PI);
 
-	modm::Location2D<int16_t> b = a.convert<int16_t>();
+	modm::Location2D<int16_t> b(a);
 
 	TEST_ASSERT_EQUALS(b.getX(), -11);
 	TEST_ASSERT_EQUALS(b.getY(), 20);
diff --git a/test/modm/math/geometry/vector2_test.cpp b/test/modm/math/geometry/vector2_test.cpp
index c788deb84b..8c12b65757 100644
--- a/test/modm/math/geometry/vector2_test.cpp
+++ b/test/modm/math/geometry/vector2_test.cpp
@@ -134,8 +134,8 @@ Vector2Test::testOperators()
 	TEST_ASSERT_EQUALS((a * 3).getY(), 5*3);
 	TEST_ASSERT_EQUALS((3 * a).getX(), 3*7);
 	TEST_ASSERT_EQUALS((3 * a).getY(), 3*5);
-	TEST_ASSERT_EQUALS((b / 2).getX(), -18/2);
-	TEST_ASSERT_EQUALS((b / 2).getY(), 4);	// 3.5 -> rounded 4
+	TEST_ASSERT_EQUALS((b / 2.0).getX(), -18/2);
+	TEST_ASSERT_EQUALS((b / 2.0).getY(), 4);	// 3.5 -> rounded 4
 
 	-b;
 	TEST_ASSERT_EQUALS(b.getX(), -18);
@@ -322,7 +322,7 @@ Vector2Test::testConversion()
 	TEST_ASSERT_EQUALS(a.getX(), 12.763f);
 	TEST_ASSERT_EQUALS(a.getY(), -13.3123f);
 
-	modm::Vector2i b = a.convert<int16_t>();
+	modm::Vector2i b(a);
 
 	TEST_ASSERT_EQUALS(b.getX(), 13);
 	TEST_ASSERT_EQUALS(b.getY(), -13);
diff --git a/test/modm/math/geometry/vector_test.cpp b/test/modm/math/geometry/vector_test.cpp
index 9e2ada7597..354533f15f 100644
--- a/test/modm/math/geometry/vector_test.cpp
+++ b/test/modm/math/geometry/vector_test.cpp
@@ -199,6 +199,24 @@ void VectorTest::testConvert()
 	modm::Vector<float, 2> f(1.3, 2.7);
 	modm::Vector<double, 2> d(1.3, 2.7);
 
+	// convert() is deprecated but we still test for b.c.
+	modm::Vector<double, 2> d2 = f.convert<double>();
+	TEST_ASSERT_EQUALS_FLOAT(d2.x, 1.3);
+	TEST_ASSERT_EQUALS_FLOAT(d2.y, 2.7);
+	// convert() is deprecated but we still test for b.c.
+	modm::Vector<float, 2> f2 = d.convert<float>();
+	TEST_ASSERT_EQUALS_FLOAT(f2.x, 1.3);
+	TEST_ASSERT_EQUALS_FLOAT(f2.y, 2.7);
+
+	// Conversion constructor replaces convert()
+	modm::Vector<double, 2> d3(f);
+	TEST_ASSERT_EQUALS_FLOAT(d3.x, 1.3);
+	TEST_ASSERT_EQUALS_FLOAT(d3.y, 2.7);
+	// Conversion constructor replaces convert()
+	modm::Vector<float, 2> f3(d);
+	TEST_ASSERT_EQUALS_FLOAT(f3.x, 1.3);
+	TEST_ASSERT_EQUALS_FLOAT(f3.y, 2.7);
+
 	modm::Vector<uint8_t, 2> u8(1.3, 2.7);
 	TEST_ASSERT_EQUALS(u8.x, 1);
 	TEST_ASSERT_EQUALS(u8.y, 3);
@@ -207,15 +225,7 @@ void VectorTest::testConvert()
 	TEST_ASSERT_EQUALS(i8.x, 1);
 	TEST_ASSERT_EQUALS(i8.y, 3);
 
-	modm::Vector<uint16_t, 2> u16(1.3, 2.7);
-	TEST_ASSERT_EQUALS(u16.x, 1);
-	TEST_ASSERT_EQUALS(u16.y, 3);
-
-	modm::Vector<int16_t, 2> i16(1.3, 2.7);
-	TEST_ASSERT_EQUALS(i16.x, 1);
-	TEST_ASSERT_EQUALS(i16.y, 3);
-
-	modm::Vector<double, 2> d2 = f.convert<double>();
-	
-	modm::Vector<float, 2> f2 = f.convert<float>();
+	modm::Vector<int8_t, 2> i82(f);
+	TEST_ASSERT_EQUALS(i82.x, 1);
+	TEST_ASSERT_EQUALS(i82.y, 3);
 }
diff --git a/test/modm/math/geometry/vector_test.hpp b/test/modm/math/geometry/vector_test.hpp
index 4ffb83e8a2..262b416b39 100644
--- a/test/modm/math/geometry/vector_test.hpp
+++ b/test/modm/math/geometry/vector_test.hpp
@@ -34,4 +34,7 @@ class VectorTest : public unittest::TestSuite
 
 	void
 	testLength();
+
+	void
+	testConvert();
 };
diff --git a/test/modm/math/matrix_test.cpp b/test/modm/math/matrix_test.cpp
index 77f5ee3737..0119c3fd11 100644
--- a/test/modm/math/matrix_test.cpp
+++ b/test/modm/math/matrix_test.cpp
@@ -44,8 +44,8 @@ MatrixTest::testConstruction()
 
 	modm::Matrix<int16_t, 3, 2> b(n);
 
-	TEST_ASSERT_EQUALS(b.getNumberOfRows(), 3);
-	TEST_ASSERT_EQUALS(b.getNumberOfColumns(), 2);
+	TEST_ASSERT_EQUALS(b.getNumberOfRows(), std::size_t(3));
+	TEST_ASSERT_EQUALS(b.getNumberOfColumns(), std::size_t(2));
 
 	TEST_ASSERT_EQUALS(b[0][0], 1);
 	TEST_ASSERT_EQUALS(b[0][1], 2);