Merge branch 'terrain-deform-2'HEADmain

2 files changed, 160 insertions, 19 deletions

diff --git a/lib/maths.h b/lib/maths.h
index 14a29d1..3959896 100644
--- a/lib/maths.h
+++ b/lib/maths.h
@@ -5,11 +5,15 @@
 #include <glm/glm.hpp>
 #include <glm/gtc/constants.hpp>
 #include <numeric>
+#include <optional>
 #include <stdexcept>
 #include <utility>
 
+template<typename T>
+concept Arithmetic = std::is_arithmetic_v<T>;
+
 struct Arc : public std::pair<Angle, Angle> {
-	template<glm::length_t Lc, glm::length_t Le, typename T, glm::qualifier Q>
+	template<glm::length_t Lc, glm::length_t Le, Arithmetic T, glm::qualifier Q>
 		requires(Lc >= 2, Le >= 2)
 	Arc(const glm::vec<Lc, T, Q> & centre, const glm::vec<Le, T, Q> & e0p, const glm::vec<Le, T, Q> & e1p) :
 		Arc {RelativePosition2D {e0p.xy() - centre.xy()}, RelativePosition2D {e1p.xy() - centre.xy()}}
@@ -118,7 +122,7 @@ namespace {
 	}
 
 	// Helper to lookup into a matrix given an xy vector coordinate
-	template<glm::length_t C, glm::length_t R, typename T, glm::qualifier Q, std::integral I = glm::length_t>
+	template<glm::length_t C, glm::length_t R, Arithmetic T, glm::qualifier Q, std::integral I = glm::length_t>
 	constexpr auto &
 	operator^(glm::mat<C, R, T, Q> & matrix, const glm::vec<2, I> rowCol)
 	{
@@ -200,7 +204,7 @@ rotate_yp(const glm::vec<2, T, Q> & angles)
 	return rotate_yp<D>(angles.y, angles.x);
 }
 
-template<glm::length_t D, glm::qualifier Q = glm::qualifier::defaultp, typename T>
+template<glm::length_t D, glm::qualifier Q = glm::qualifier::defaultp, std::floating_point T>
 	requires(D >= 2)
 constexpr auto
 vector_yaw(const glm::vec<D, T, Q> & diff)
@@ -208,7 +212,7 @@ vector_yaw(const glm::vec<D, T, Q> & diff)
 	return std::atan2(diff.x, diff.y);
 }
 
-template<glm::length_t D, glm::qualifier Q = glm::qualifier::defaultp, typename T>
+template<glm::length_t D, glm::qualifier Q = glm::qualifier::defaultp, std::floating_point T>
 	requires(D >= 3)
 constexpr auto
 vector_pitch(const glm::vec<D, T, Q> & diff)
@@ -216,7 +220,7 @@ vector_pitch(const glm::vec<D, T, Q> & diff)
 	return std::atan(diff.z);
 }
 
-template<typename T, glm::qualifier Q>
+template<std::floating_point T, glm::qualifier Q>
 constexpr glm::vec<2, T, Q>
 vector_normal(const glm::vec<2, T, Q> & vector)
 {
@@ -230,7 +234,7 @@ round_frac(const T value, const T frac)
 	return std::round(value / frac) * frac;
 }
 
-template<typename T>
+template<Arithmetic T>
 	requires requires(T value) { value * value; }
 constexpr auto
 sq(T value)
@@ -263,14 +267,15 @@ crossProduct(const glm::vec<3, T, Q> & valueA, const glm::vec<3, T, Q> & valueB)
 	return glm::cross(valueA, valueB);
 }
 
-template<typename R = float, typename Ta, typename Tb>
+template<Arithmetic R = float, Arithmetic Ta, Arithmetic Tb>
 constexpr auto
 ratio(const Ta valueA, const Tb valueB)
 {
-	return (static_cast<R>(valueA) / static_cast<R>(valueB));
+	using Common = std::common_type_t<Ta, Ta>;
+	return static_cast<R>((static_cast<Common>(valueA) / static_cast<Common>(valueB)));
 }
 
-template<typename R = float, typename T, glm::qualifier Q>
+template<Arithmetic R = float, Arithmetic T, glm::qualifier Q>
 constexpr auto
 ratio(const glm::vec<2, T, Q> & value)
 {
@@ -284,14 +289,14 @@ perspective_divide(const glm::vec<4, T, Q> & value)
 	return value / value.w;
 }
 
-template<glm::length_t L1, glm::length_t L2, typename T, glm::qualifier Q>
+template<glm::length_t L1, glm::length_t L2, Arithmetic T, glm::qualifier Q>
 constexpr glm::vec<L1 + L2, T, Q>
 operator||(const glm::vec<L1, T, Q> valueA, const glm::vec<L2, T, Q> valueB)
 {
 	return {valueA, valueB};
 }
 
-template<glm::length_t L, typename T, glm::qualifier Q>
+template<glm::length_t L, Arithmetic T, glm::qualifier Q>
 constexpr glm::vec<L + 1, T, Q>
 operator||(const glm::vec<L, T, Q> valueA, const T valueB)
 {
@@ -326,7 +331,35 @@ normalize(T ang)
 	return ang;
 }
 
-template<typename T, glm::qualifier Q>
+template<Arithmetic T> using CalcType = std::conditional_t<std::is_floating_point_v<T>, T, int64_t>;
+
+template<Arithmetic T, glm::qualifier Q = glm::defaultp>
+[[nodiscard]] constexpr std::optional<glm::vec<2, T, Q>>
+linesIntersectAt(const glm::vec<2, T, Q> Aabs, const glm::vec<2, T, Q> Babs, const glm::vec<2, T, Q> Cabs,
+		const glm::vec<2, T, Q> Dabs)
+{
+	using CT = CalcType<T>;
+	using CVec = glm::vec<2, CT, Q>;
+	// Line AB represented as a1x + b1y = c1
+	const CVec Brel = Babs - Aabs;
+	const CT a1 = Brel.y;
+	const CT b1 = -Brel.x;
+
+	// Line CD represented as a2x + b2y = c2
+	const CVec Crel = Cabs - Aabs, Del = Dabs - Aabs;
+	const CT a2 = Del.y - Crel.y;
+	const CT b2 = Crel.x - Del.x;
+	const CT c2 = (a2 * Crel.x) + (b2 * Crel.y);
+
+	const auto determinant = (a1 * b2) - (a2 * b1);
+
+	if (determinant == 0) {
+		return std::nullopt;
+	}
+	return Aabs + CVec {(b1 * c2) / -determinant, (a1 * c2) / determinant};
+}
+
+template<Arithmetic T, glm::qualifier Q>
 std::pair<glm::vec<2, T, Q>, bool>
 find_arc_centre(glm::vec<2, T, Q> start, Rotation2D startDir, glm::vec<2, T, Q> end, Rotation2D endDir)
 {
@@ -339,7 +372,7 @@ find_arc_centre(glm::vec<2, T, Q> start, Rotation2D startDir, glm::vec<2, T, Q>
 	throw std::runtime_error("no intersection");
 }
 
-template<typename T, glm::qualifier Q>
+template<Arithmetic T, glm::qualifier Q>
 std::pair<glm::vec<2, T, Q>, bool>
 find_arc_centre(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, Angle entrye)
 {
@@ -349,7 +382,7 @@ find_arc_centre(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, An
 	return find_arc_centre(start, sincos(entrys + half_pi), end, sincos(entrye - half_pi));
 }
 
-template<typename T, glm::qualifier Q>
+template<Arithmetic T, glm::qualifier Q>
 Angle
 find_arcs_radius(glm::vec<2, T, Q> start, Rotation2D ad, glm::vec<2, T, Q> end, Rotation2D bd)
 {
@@ -374,7 +407,7 @@ find_arcs_radius(glm::vec<2, T, Q> start, Rotation2D ad, glm::vec<2, T, Q> end,
 			/ (2 * (sq(X) - 2 * X * Z + sq(Z) + sq(Y) - 2 * Y * W + sq(W) - 4));
 }
 
-template<typename T, glm::qualifier Q>
+template<Arithmetic T, glm::qualifier Q>
 std::pair<Angle, Angle>
 find_arcs_radius(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, Angle entrye)
 {
@@ -384,7 +417,7 @@ find_arcs_radius(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, A
 	return {getrad(-half_pi), getrad(half_pi)};
 }
 
-template<typename T>
+template<Arithmetic T>
 auto
 midpoint(const std::pair<T, T> & v)
 {
@@ -392,7 +425,7 @@ midpoint(const std::pair<T, T> & v)
 }
 
 // std::pow is not constexpr
-template<typename T>
+template<Arithmetic T>
 	requires requires(T n) { n *= n; }
 constexpr T
 pow(const T base, std::integral auto exp)
@@ -405,14 +438,14 @@ pow(const T base, std::integral auto exp)
 }
 
 // Conversions
-template<typename T>
+template<Arithmetic T>
 constexpr auto
 mph_to_ms(T v)
 {
 	return v / 2.237L;
 }
 
-template<typename T>
+template<Arithmetic T>
 constexpr auto
 kph_to_ms(T v)
 {
diff --git a/lib/triangle.h b/lib/triangle.h
new file mode 100644
index 0000000..812bfab
--- /dev/null
+++ b/lib/triangle.h
@@ -0,0 +1,108 @@
+#pragma once
+
+#include "config/types.h"
+#include "maths.h"
+#include <glm/glm.hpp>
+
+template<glm::length_t Dim, Arithmetic T, glm::qualifier Q = glm::defaultp>
+struct Triangle : public glm::vec<3, glm::vec<Dim, T, Q>> {
+	using Point = glm::vec<Dim, T, Q>;
+	using Base = glm::vec<3, glm::vec<Dim, T, Q>>;
+	using Base::Base;
+
+	[[nodiscard]] constexpr Point
+	operator*(BaryPosition bari) const
+	{
+		return p(0) + (sideDifference(1) * bari.x) + (sideDifference(2) * bari.y);
+	}
+
+	[[nodiscard]] constexpr Point
+	centroid() const
+	{
+		return [this]<glm::length_t... Axis>(std::integer_sequence<glm::length_t, Axis...>) {
+			return Point {(p(0)[Axis] + p(1)[Axis] + p(2)[Axis]) / 3 ...};
+		}(std::make_integer_sequence<glm::length_t, Dim>());
+	}
+
+	[[nodiscard]] constexpr auto
+	area() const
+		requires(Dim == 3)
+	{
+		return glm::length(crossProduct(sideDifference(1), sideDifference(2))) / T {2};
+	}
+
+	[[nodiscard]] constexpr Normal3D
+	normal() const
+		requires(Dim == 3)
+	{
+		return crossProduct(sideDifference(1), sideDifference(2));
+	}
+
+	[[nodiscard]] constexpr Normal3D
+	nnormal() const
+		requires(Dim == 3)
+	{
+		return glm::normalize(normal());
+	}
+
+	[[nodiscard]] constexpr auto
+	sideDifference(glm::length_t side) const
+	{
+		return difference(p(side), p(0));
+	}
+
+	[[nodiscard]] constexpr auto
+	angle(glm::length_t corner) const
+	{
+		return Arc {P(corner), P(corner + 2), P(corner + 1)}.length();
+	}
+
+	template<glm::length_t D = Dim>
+	[[nodiscard]] constexpr auto
+	angleAt(const glm::vec<D, T, Q> pos) const
+		requires(D <= Dim)
+	{
+		for (glm::length_t i {}; i < 3; ++i) {
+			if (glm::vec<D, T, Q> {p(i)} == pos) {
+				return angle(i);
+			}
+		}
+		return 0.F;
+	}
+
+	[[nodiscard]] constexpr auto
+	p(const glm::length_t idx) const
+	{
+		return Base::operator[](idx);
+	}
+
+	[[nodiscard]] constexpr auto
+	P(const glm::length_t idx) const
+	{
+		return Base::operator[](idx % 3);
+	}
+
+	[[nodiscard]] constexpr Point *
+	begin()
+	{
+		return &(Base::x);
+	}
+
+	[[nodiscard]] constexpr const Point *
+	begin() const
+	{
+		return &(Base::x);
+	}
+
+	[[nodiscard]] constexpr Point *
+	end()
+	{
+		return begin() + 3;
+	}
+
+	[[nodiscard]] constexpr const Point *
+	end() const
+	{
+		return begin() + 3;
+	}
+};