Add all the files

src/util/math/glm.h

@@ -0,0 +1,36 @@
+#pragma once
+
+namespace glm
+{
+	inline quat normalize_xyz(const quat& q)
+	{
+		const auto xyzTargetLength = std::sqrt(1.0f - q.w * q.w);
+		const auto lengthScaler = xyzTargetLength / sqrtf(q.x * q.x + q.y * q.y + q.z * q.z);
+		return quat(q.w, q.x * lengthScaler, q.y * lengthScaler, q.z * lengthScaler);
+	}
+
+	inline vec3 GetVectorX(const quat& q)
+	{
+		return vec3(
+			2.0f * (q.w * q.w + q.x * q.x) - 1.0f,
+			2.0f * (q.x * q.y - q.w * q.z),
+			2.0f * (q.x * q.z + q.w * q.y));
+	}
+
+	inline vec3 GetVectorY(const quat& q)
+	{
+		return vec3(
+			2.0f * (q.x * q.y + q.w * q.z),
+			2.0f * (q.w * q.w + q.y * q.y) - 1.0f,
+			2.0f * (q.y * q.z - q.w * q.x)
+		);
+	}
+
+	inline vec3 GetVectorZ(const quat& q)
+	{
+		return vec3 (
+			2.0f * (q.x * q.z - q.w * q.y),
+			2.0f * (q.y * q.z + q.w * q.x),
+			2.0f * (q.w * q.w + q.z * q.z) - 1.0f);
+	}
+}

src/util/math/quaternion.h

@@ -0,0 +1,217 @@
+#pragma once
+
+#include <tuple>
+#include <wx/math.h>
+#include "util/math/vector3.h"
+#define DEG2RAD(__d__) ((__d__ * M_PI) / 180.0f )
+#define RAD2DEG(__r__) ((__r__ * 180.0f) / M_PI)
+
+
+template<typename T> 
+class Quaternion
+{
+public:
+	T w;
+	T x;
+	T y;
+	T z;
+
+	Quaternion();
+	Quaternion(const T& w, const T& x, const T& y, const T& z);
+	Quaternion(float x, float y, float z);
+
+	void Assign(float w, float x, float y, float z)
+	{
+		this->w = w;
+		this->x = x;
+		this->y = y;
+		this->z = z;
+	}
+
+	static Quaternion FromAngleAxis(float inAngle, float inX, float inY, float inZ);
+
+	std::tuple<Vector3<T>, Vector3<T>, Vector3<T>> GetRotationMatrix();
+	std::tuple<Vector3<T>, Vector3<T>, Vector3<T>> GetTransposedRotationMatrix();
+
+	// normalize but keep W
+	void NormalizeXYZ()
+	{
+		const T xyzTargetLength = sqrtf((T)1.0 - w * w);
+		const T lengthScaler = xyzTargetLength / sqrtf(x * x + y * y + z * z);
+		x *= lengthScaler;
+		y *= lengthScaler;
+		z *= lengthScaler;
+	}
+
+	void NormalizeXYZW()
+	{
+		const T lengthScaler = 1.0f / sqrtf(w * w + x * x + y * y + z * z);
+		w *= lengthScaler;
+		x *= lengthScaler;
+		y *= lengthScaler;
+		z *= lengthScaler;
+	}
+
+	Vector3<T> GetVectorX() const
+	{
+		return Vector3<T>(
+			2.0f * (w * w + x * x) - 1.0f,
+			2.0f * (x * y - w * z),
+			2.0f * (x * z + w * y));
+	}
+
+	Vector3<T> GetVectorY() const
+	{
+		return Vector3<T>(
+			2.0f * (x * y + w * z),
+			2.0f * (w * w + y * y) - 1.0f,
+			2.0f * (y * z - w * x)
+			);
+	}
+
+	Vector3<T> GetVectorZ() const
+	{
+		return Vector3<T>(
+			2.0f * (x * z - w * y),
+			2.0f * (y * z + w * x),
+			2.0f * (w * w + z * z) - 1.0f);
+	}
+
+	Quaternion& operator*=(const Quaternion<T>& rhs)
+	{
+		Assign(w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z,
+			w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y,
+			w * rhs.y - x * rhs.z + y * rhs.w + z * rhs.x,
+			w * rhs.z + x * rhs.y - y * rhs.x + z * rhs.w);
+		return *this;
+	}
+
+	Quaternion& operator+=(const Quaternion<T>& rhs)
+	{
+		w += rhs.w;
+		x += rhs.x;
+		y += rhs.y;
+		z += rhs.z;
+		return *this;
+	}
+
+	friend Quaternion operator*(Quaternion<T> lhs, const Quaternion<T>& rhs)
+	{
+		lhs *= rhs;
+		return lhs;
+	}
+
+};
+
+template <typename T>
+Quaternion<T>::Quaternion() 
+	: w(), x(), y(), z()
+{}
+
+template <typename T>
+Quaternion<T>::Quaternion(const T& w, const T& x, const T& y, const T& z)
+	: w(w), x(x), y(y), z(z)
+{}
+
+template <typename T>
+Quaternion<T>::Quaternion(float x, float y, float z)
+{
+	float pitch = DEG2RAD(x);
+	float yaw = DEG2RAD(y);
+	float roll = DEG2RAD(z);
+
+	float cyaw = cos(0.5f * yaw);
+	float cpitch = cos(0.5f * pitch);
+	float croll = cos(0.5f * roll);
+	float syaw = sin(0.5f * yaw);
+	float spitch = sin(0.5f * pitch);
+	float sroll = sin(0.5f * roll);
+
+	float cyawcpitch = cyaw * cpitch;
+	float syawspitch = syaw * spitch;
+	float cyawspitch = cyaw * spitch;
+	float syawcpitch = syaw * cpitch;
+
+	this->w = cyawcpitch * croll + syawspitch * sroll;
+	this->x = cyawspitch * croll + syawcpitch * sroll;
+	this->y = syawcpitch * croll - cyawspitch * sroll;
+	this->z = cyawcpitch * sroll - syawspitch * croll;
+}
+
+template<typename T>
+Quaternion<T> Quaternion<T>::FromAngleAxis(float inAngle, float inX, float inY, float inZ)
+{
+	Quaternion<T> result = Quaternion<T>(cosf(inAngle * 0.5f), inX, inY, inZ);
+	result.NormalizeXYZ();
+	return result;
+}
+
+template <typename T>
+std::tuple<Vector3<T>, Vector3<T>, Vector3<T>> Quaternion<T>::GetRotationMatrix()
+{
+	float sqw = w*w;
+	float sqx = x*x;
+	float sqy = y*y;
+	float sqz = z*z;
+
+	// invs (inverse square length) is only required if quaternion is not already normalised
+	float invs = 1.0f / (sqx + sqy + sqz + sqw);
+	
+	Vector3<T> v1, v2, v3;
+	v1.x = (sqx - sqy - sqz + sqw) * invs; // since sqw + sqx + sqy + sqz =1/invs*invs
+	v2.y = (-sqx + sqy - sqz + sqw) * invs;
+	v3.z = (-sqx - sqy + sqz + sqw) * invs;
+
+	float tmp1 = x*y;
+	float tmp2 = z*w;
+	v2.x = 2.0 * (tmp1 + tmp2)*invs;
+	v1.y = 2.0 * (tmp1 - tmp2)*invs;
+
+	tmp1 = x*z;
+	tmp2 = y*w;
+	v3.x = 2.0 * (tmp1 - tmp2)*invs;
+	v1.z = 2.0 * (tmp1 + tmp2)*invs;
+
+	tmp1 = y*z;
+	tmp2 = x*w;
+	v3.y = 2.0 * (tmp1 + tmp2)*invs;
+	v2.z = 2.0 * (tmp1 - tmp2)*invs;
+
+	return std::make_tuple(v1, v2, v3);
+}
+
+template <typename T>
+std::tuple<Vector3<T>, Vector3<T>, Vector3<T>> Quaternion<T>::GetTransposedRotationMatrix()
+{
+	float sqw = w*w;
+	float sqx = x*x;
+	float sqy = y*y;
+	float sqz = z*z;
+
+	// invs (inverse square length) is only required if quaternion is not already normalised
+	float invs = 1.0f / (sqx + sqy + sqz + sqw);
+
+	Vector3<T> v1, v2, v3;
+	v1.x = (sqx - sqy - sqz + sqw) * invs; // since sqw + sqx + sqy + sqz =1/invs*invs
+	v2.y = (-sqx + sqy - sqz + sqw) * invs;
+	v3.z = (-sqx - sqy + sqz + sqw) * invs;
+
+	float tmp1 = x*y;
+	float tmp2 = z*w;
+	v1.y = 2.0 * (tmp1 + tmp2)*invs;
+	v2.x = 2.0 * (tmp1 - tmp2)*invs;
+
+	tmp1 = x*z;
+	tmp2 = y*w;
+	v1.z = 2.0 * (tmp1 - tmp2)*invs;
+	v3.x = 2.0 * (tmp1 + tmp2)*invs;
+
+	tmp1 = y*z;
+	tmp2 = x*w;
+	v2.z = 2.0 * (tmp1 + tmp2)*invs;
+	v3.y = 2.0 * (tmp1 - tmp2)*invs;
+
+	return std::make_tuple(v1, v2, v3);
+}
+
+using Quaternionf = Quaternion<float>;

src/util/math/vector2.h

@@ -0,0 +1,150 @@
+#pragma once
+#include <cassert>
+
+template <typename T>
+class Vector2
+{
+public:
+	T x;
+	T y;
+
+	Vector2()
+		: x{}, y{} {}
+
+	Vector2(T x, T y)
+		: x(x), y(y) {}
+
+	template <typename U = T>
+	Vector2(const Vector2<U>& v)
+		: x((T)v.x), y((T)v.y) {}
+
+	float Length() const
+	{
+		return (float)std::sqrt((x * x) + (y * y));
+	}
+
+	float Cross(const Vector2& v) const
+	{
+		return x * v.y - y * v.x;
+	}
+
+	float Dot(const Vector2& v) const
+	{
+		return x * v.x + y * v.y;
+	}
+
+	Vector2 Ortho() const
+	{
+		return Vector2(y, -x);
+	}
+
+	Vector2 Normalized() const
+	{
+		const auto len = Length();
+		if (len == 0)
+			return Vector2<T>();
+
+		return Vector2<T>((T)((float)x / len), (T)((float)y / len));
+	}
+
+	Vector2& Normalize()
+	{
+		const auto len = Length();
+		if (len != 0)
+		{
+			x = (T)((float)x / len);
+			y = (T)((float)y / len);
+		}
+
+		return *this;
+	}
+
+	static Vector2 Max(const Vector2& v1, const Vector2& v2)
+	{
+		return Vector2(std::max(v1.x, v2.x), std::max(v1.y, v2.y));
+	}
+
+	static Vector2 Min(const Vector2& v1, const Vector2& v2)
+	{
+		return Vector2(std::min(v1.x, v2.x), std::min(v1.y, v2.y));
+	}
+
+	bool operator==(const Vector2& v) const
+	{
+		return x == v.x && y == v.y;
+	}
+	bool operator!=(const Vector2& v) const
+	{
+		return !(*this == v);
+	}
+
+	Vector2& operator+=(const Vector2& v)
+	{
+		x += v.x;
+		y += v.y;
+		return *this;
+	}
+	Vector2& operator-=(const Vector2& v)
+	{
+		x -= v.x;
+		y -= v.y;
+		return *this;
+	}
+
+	Vector2 operator+(const Vector2& v) const
+	{
+		return Vector2(x + v.x, y + v.y);
+	}
+	Vector2 operator-(const Vector2& v) const
+	{
+		return Vector2(x - v.x, y - v.y);
+	}
+
+	Vector2& operator+=(const T& v)
+	{
+		x += v;
+		y += v;
+		return *this;
+	}
+	Vector2& operator-=(const T& v)
+	{
+		x -= v;
+		y -= v;
+		return *this;
+	}
+	Vector2& operator*=(const T& v)
+	{
+		x *= v;
+		y *= v;
+		return *this;
+	}
+	Vector2& operator/=(const T& v)
+	{
+		assert(v != 0);
+		x /= v;
+		y /= v;
+		return *this;
+	}
+
+	Vector2 operator+(const T& v)
+	{
+		return Vector2(x + v, y + v);
+	}
+	Vector2 operator-(const T& v)
+	{
+		return Vector2(x - v, y - v);
+	}
+	Vector2 operator*(const T& v)
+	{
+		return Vector2(x * v, y * v);
+	}
+	Vector2 operator/(const T& v)
+	{
+		assert(v != 0);
+		return Vector2(x / v, y / v);
+	}
+};
+
+using Vector2f = Vector2<float>;
+using Vector2i = Vector2<int>;
+

src/util/math/vector3.h

@@ -0,0 +1,233 @@
+#pragma once
+#include <cassert>
+
+template <typename T>
+class Vector3 {
+public:
+	T x;
+	T y;
+	T z;
+
+	Vector3() : x{}, y{}, z{} {}
+	Vector3(T x, T y, T z) : x(x), y(y), z(z) {}
+
+	template <typename U=T>
+	Vector3(const Vector3<U>& v)
+		: x((T)v.x), y((T)v.y), z((T)v.z) {}
+
+	float Length() const
+	{
+		return std::sqrt((x * x) + (y * y) + (z * z));
+	}
+
+	float Dot(const Vector3& v) const
+	{
+		return x * v.x + y * v.y + z * v.z;
+	}
+
+	Vector3 Cross(const Vector3& v) const
+	{
+		return Vector3(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
+	}
+
+	Vector3 Normalized() const
+	{
+		const auto len = Length();
+		if (len == 0)
+			return {};
+
+		return *this / len;
+	}
+
+	Vector3& Normalize()
+	{
+		const auto len = Length();
+		if (len != 0) 
+			*this /= len;
+
+		return *this;
+	}
+
+	//Vector3& Scale(const Vector3& v)
+	//{
+	//	*this *= v;
+	//	return *this;
+	//}
+
+	void Scale(const float s)
+	{
+		this->x *= s;
+		this->y *= s;
+		this->z *= s;
+	}
+
+	Vector3& RotateX(float theta);
+	Vector3& RotateY(float theta);
+	Vector3& RotateZ(float theta);
+
+
+	bool operator==(const Vector3& v)
+	{
+		return x == v.x && y == v.y && z == v.z;
+	}
+
+	bool operator!=(const Vector3& v)
+	{
+		return !(*this == v);
+	}
+
+	template <typename U = T>
+	Vector3& operator+=(const Vector3<U>& v)
+	{
+		x += (T)v.x;
+		y += (T)v.y;
+		z += (T)v.z;
+		return *this;
+	}
+
+	template <typename U = T>
+	Vector3& operator-=(const Vector3<U>& v)
+	{
+		x -= (T)v.x;
+		y -= (T)v.y;
+		z -= (T)v.z;
+		return *this;
+	}
+
+	template <typename U = T>
+	Vector3& operator*=(const Vector3<U>& v)
+	{
+		x *= (T)v.x;
+		y *= (T)v.y;
+		z *= (T)v.z;
+		return *this;
+	}
+
+	template <typename U = T>
+	Vector3& operator/=(const Vector3<U>& v)
+	{
+		assert(v.x != 0 && v.y != 0 && v.z != 0);
+		x /= (T)v.x;
+		y /= (T)v.y;
+		z /= (T)v.z;
+		return *this;
+	}
+
+	template <typename U = T>
+	Vector3 operator+(const Vector3<U>& v) const
+	{
+		return Vector3(x + (T)v.x, y + (T)v.y, z + (T)v.z);
+	}
+
+	template <typename U = T>
+	Vector3 operator-(const Vector3<U>& v) const
+	{
+		return Vector3(x - (T)v.x, y - (T)v.y, z - (T)v.z);
+	}
+
+	template <typename U = T>
+	Vector3 operator*(const Vector3<U>& v) const
+	{
+		return Vector3(x * (T)v.x, y * (T)v.y, z * (T)v.z);
+	}
+
+	template <typename U = T>
+	Vector3 operator/(const Vector3<U>& v) const
+	{
+		assert(v.x != 0 && v.y != 0 && v.z != 0);
+		return Vector3(x / (T)v.x, y / (T)v.y, z / (T)v.z);
+	}
+
+	Vector3& operator+=(T v)
+	{
+		x += v;
+		y += v;
+		z += v;
+		return *this;
+	}
+	Vector3& operator-=(T v)
+	{
+		x -= v;
+		y -= v;
+		z -= v;
+		return *this;
+	}
+	Vector3& operator*=(T v)
+	{
+		x *= v;
+		y *= v;
+		z *= v;
+		return *this;
+	}
+	Vector3& operator/=(T v)
+	{
+		assert(v != 0);
+		x /= v;
+		y /= v;
+		z /= v;
+		return *this;
+	}
+
+	Vector3 operator+(T v) const
+	{
+		return Vector3(x + v, y + v, z + v);
+	}
+	Vector3 operator-(T v) const
+	{
+		return Vector3(x - v, y - v, z - v);
+	}
+	Vector3 operator*(T v) const
+	{
+		return Vector3(x * v, y * v, z * v);
+	}
+	Vector3 operator/(T v) const
+	{
+		assert(v != 0);
+		return Vector3(x / (T)v, y / (T)v, z / (T)v);
+	}
+
+	bool IsZero() const
+	{
+		return x == 0 && y == 0 && z == 0;
+	}
+	bool HasZero() const
+	{
+		return x == 0 || y == 0 || z == 0;
+	}
+};
+
+
+
+template <typename T>
+Vector3<T>& Vector3<T>::RotateX(float theta)
+{
+	const float sin = std::sin(theta);
+	const float cos = std::cos(theta);
+	y = y * cos - z * sin;
+	z = y * sin + z * cos;
+	return *this;
+}
+
+template <typename T>
+Vector3<T>& Vector3<T>::RotateY(float theta)
+{
+	const float sin = std::sin(theta);
+	const float cos = std::cos(theta);
+	x = x * cos + z * sin;
+	z = -x * sin + z * cos;
+	return *this;
+}
+
+template <typename T>
+Vector3<T>& Vector3<T>::RotateZ(float theta)
+{
+	const float sin = std::sin(theta);
+	const float cos = std::cos(theta);
+	x = x * cos - y * sin;
+	y = x * sin + y * cos;
+	return *this;
+}
+
+
+using Vector3f = Vector3<float>;
+using Vector3i = Vector3<int>;