3 files changed, 104 insertions, 0 deletions
diff --git a/utility/maths.cpp b/utility/maths.cpp
new file mode 100644
index 0000000..a3315a3
--- /dev/null
+++ b/utility/maths.cpp
@@ -0,0 +1,54 @@
+#include "maths.h"
+#include <cmath>
+#include <glm/glm.hpp>
+#include <glm/gtx/rotate_vector.hpp>
+#include <glm/gtx/transform.hpp>
+#include <glm/gtx/vector_angle.hpp>
+
+glm::mat4
+flat_orientation(const glm::vec3 & diff)
+{
+	static const auto oneeighty {glm::rotate(pi, up)};
+	const auto flatdiff {glm::normalize(glm::vec3 {diff.x, 0, diff.z})};
+	auto e {glm::orientation(flatdiff, north)};
+	// Handle if diff is exactly opposite to north
+	return (std::isnan(e[0][0])) ? oneeighty : e;
+}
+
+float
+flat_angle(const glm::vec3 & diff)
+{
+	const auto flatdiff {glm::normalize(glm::vec3 {diff.x, 0, diff.z})};
+	return glm::orientedAngle(flatdiff, north, up);
+}
+
+float
+round_frac(const float & v, const float & frac)
+{
+	return std::round(v / frac) * frac;
+}
+
+float
+normalize(float ang)
+{
+	while (ang > pi) {
+		ang -= two_pi;
+	}
+	while (ang <= -pi) {
+		ang += two_pi;
+	}
+	return ang;
+}
+
+Arc
+create_arc(const glm::vec3 & centre3, const glm::vec3 & e0p, const glm::vec3 & e1p)
+{
+	const auto diffa = centre3 - e0p;
+	const auto diffb = centre3 - e1p;
+	const auto anga = flat_angle(diffa);
+	const auto angb = [&diffb, &anga]() {
+		const auto angb = flat_angle(diffb);
+		return (angb < anga) ? angb + two_pi : angb;
+	}();
+	return {anga, angb};
+}
diff --git a/utility/maths.h b/utility/maths.h
new file mode 100644
index 0000000..0b379d0
--- /dev/null
+++ b/utility/maths.h
@@ -0,0 +1,38 @@
+#ifndef MATH_H
+#define MATH_H
+
+#include <glm/glm.hpp>
+#include <glm/gtc/constants.hpp>
+#include <utility>
+
+using Arc = std::pair<float, float>;
+
+constexpr const glm::vec3 up {0, 1, 0};
+constexpr const glm::vec3 north {0, 0, 1};
+constexpr auto half_pi {glm::half_pi<float>()};
+constexpr auto pi {glm::pi<float>()};
+constexpr auto two_pi {glm::two_pi<float>()};
+
+glm::mat4 flat_orientation(const glm::vec3 & diff);
+
+float flat_angle(const glm::vec3 & diff);
+
+float round_frac(const float & v, const float & frac);
+
+constexpr inline glm::vec3
+operator!(const glm::vec2 & v)
+{
+	return {v.x, 0, v.y};
+}
+
+constexpr inline float
+arc_length(const Arc & arc)
+{
+	return arc.second - arc.first;
+}
+
+float normalize(float ang);
+
+Arc create_arc(const glm::vec3 & centre3, const glm::vec3 & e0p, const glm::vec3 & e1p);
+
+#endif
diff --git a/utility/sorting.hpp b/utility/sorting.hpp
new file mode 100644
index 0000000..0cb0eaf
--- /dev/null
+++ b/utility/sorting.hpp
@@ -0,0 +1,12 @@
+#ifndef SORTING_H
+#define SORTING_H
+
+template<typename T> struct PtrSorter {
+	bool
+	operator()(const T & a, const T & b) const
+	{
+		return *a < *b;
+	}
+};
+
+#endif