From 68832526efafb4db60e5627eba6a9051585a4eb9 Mon Sep 17 00:00:00 2001
From: Dan Goodliffe <dan@randomdan.homeip.net>
Date: Sun, 7 Feb 2021 12:04:33 +0000
Subject: Factor a lot of maths and stuff into utility

---
 utility/maths.cpp | 54 ++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 54 insertions(+)
 create mode 100644 utility/maths.cpp

(limited to 'utility/maths.cpp')

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};
+}
-- 
cgit v1.2.3