6 files changed, 96 insertions, 0 deletions

diff --git a/application/main.cpp b/application/main.cpp
index 69b62ba..49b7cda 100644
--- a/application/main.cpp
+++ b/application/main.cpp
@@ -67,6 +67,13 @@ public:
 		{
 			const glm::vec3 j {-1100, 15, -1100}, k {-1100, 15, -1000}, l {-1150, 10, -1050}, m {-1050, 10, -1050};
 			rl->addLink<RailLinkStraight>(j, k);
+			auto e = rl->addLinksBetween(k, {-1000, 20, -800})->ends[1].first->pos;
+			e = rl->addLinksBetween(e, {-900, 30, -600})->ends[0].first->pos;
+			e = rl->addLinksBetween(e, {-600, 32, -500})->ends[1].first->pos;
+			e = rl->addLinksBetween(e, {-500, 30, -800})->ends[1].first->pos;
+			e = rl->addLinksBetween(e, {-600, 25, -900})->ends[1].first->pos;
+			e = rl->addLinksBetween(e, {-1000, 10, -1129})->ends[0].first->pos;
+			e = rl->addLinksBetween(e, j)->ends[0].first->pos;
 			rl->addLink<RailLinkCurve>(l, k, glm::vec2 {l.x, k.z});
 			auto l3 = rl->addLink<RailLinkStraight>(l, m);
 			rl->addLink<RailLinkCurve>(m, j, glm::vec2 {m.x, j.z});
diff --git a/game/network/rail.cpp b/game/network/rail.cpp
index bf1d71d..c1a72fb 100644
--- a/game/network/rail.cpp
+++ b/game/network/rail.cpp
@@ -10,6 +10,7 @@
 #include <glm/gtx/transform.hpp>
 #include <initializer_list>
 #include <maths.h>
+#include <stdexcept>
 #include <type_traits>
 #include <utility>
 
@@ -36,6 +37,44 @@ RailLinks::joinLinks(const LinkPtr & l) const
 	}
 }
 
+std::shared_ptr<RailLink>
+RailLinks::addLinksBetween(glm::vec3 start, glm::vec3 end)
+{
+	auto node1ins = nodes.insert(std::make_shared<Node>(start));
+	auto node2ins = nodes.insert(std::make_shared<Node>(end));
+	if (node1ins.second && node2ins.second) {
+		// Both nodes are new, direct link, easy
+		return addLink<RailLinkStraight>(start, end);
+	}
+	if (node1ins.second && !node2ins.second) {
+		// node1 is new, node2 exists, but we build from existing outwards
+		std::swap(node1ins, node2ins);
+		std::swap(start, end);
+	}
+	// Find start link/end - opposite entry dir to existing link; so pi +...
+	float dir = pi + [this](const auto & n) {
+		for (const auto & l : links.objects) {
+			for (const auto & e : l->ends) {
+				if (e.first == n) {
+					return e.second;
+				}
+			}
+		}
+		throw std::runtime_error("Node exists but couldn't find it");
+	}(*node1ins.first);
+	const glm::vec2 flatStart {start.x, start.z}, flatEnd {end.x, end.z};
+	// if (node2ins.second) { // Unimplemented second arc/stright required
+	const auto diff {end - start};
+	const auto vy {vector_yaw(diff)};
+	const auto n2ed {(vy * 2) - dir - pi};
+	const auto centre {find_arc_centre(flatStart, dir, flatEnd, n2ed)};
+
+	if (centre.second) { // right hand arc
+		std::swap(start, end);
+	}
+	return addLink<RailLinkCurve>(start, end, centre.first);
+}
+
 void
 RailLinks::render(const Shader & shader) const
 {
diff --git a/game/network/rail.h b/game/network/rail.h
index ac944f3..5837ac1 100644
--- a/game/network/rail.h
+++ b/game/network/rail.h
@@ -71,6 +71,8 @@ public:
 		return l;
 	}
 
+	std::shared_ptr<RailLink> addLinksBetween(glm::vec3 start, glm::vec3 end);
+
 private:
 	using Nodes = std::set<NodePtr, PtrSorter<NodePtr>>;
 	Collection<RailLink> links;
diff --git a/test/test-maths.cpp b/test/test-maths.cpp
index 49114da..d5848c6 100644
--- a/test/test-maths.cpp
+++ b/test/test-maths.cpp
@@ -2,6 +2,7 @@
 
 #include <boost/test/data/test_case.hpp>
 #include <boost/test/unit_test.hpp>
+#include <iomanip>
 #include <stream_support.hpp>
 
 #include <glm/glm.hpp>
@@ -74,3 +75,20 @@ BOOST_DATA_TEST_CASE(test_create_arc,
 	BOOST_CHECK_CLOSE(arc.first, a.first, 1.F);
 	BOOST_CHECK_CLOSE(arc.second, a.second, 1.F);
 }
+
+using fac = std::tuple<glm::vec2, float, glm::vec2, float, glm::vec2, bool>;
+BOOST_DATA_TEST_CASE(test_find_arc_centre,
+		boost::unit_test::data::make<fac>({
+				{{2, 2}, pi, {3, 3}, half_pi, {3, 2}, true},
+				{{2, 2}, pi, {1, 3}, -half_pi, {1, 2}, false},
+				{{-1100, -1000}, pi, {-900, -800}, half_pi, {-900, -1000}, true},
+				{{1100, 1000}, 0, {1050, 900}, pi + 0.92, {973, 1000}, true},
+				{{1050, 900}, 0.92, {1000, 800}, pi, {1127, 800}, false},
+		}),
+		s, es, e, ee, exp, lr)
+{
+	const auto c = find_arc_centre(s, es, e, ee);
+	BOOST_CHECK_CLOSE(exp.x, c.first.x, 1);
+	BOOST_CHECK_CLOSE(exp.y, c.first.y, 1);
+	BOOST_CHECK_EQUAL(lr, c.second);
+}
diff --git a/utility/maths.cpp b/utility/maths.cpp
index 0bd3eac..74a3085 100644
--- a/utility/maths.cpp
+++ b/utility/maths.cpp
@@ -3,6 +3,8 @@
 #include <glm/glm.hpp>
 #include <glm/gtx/rotate_vector.hpp>
 #include <glm/gtx/transform.hpp>
+#include <initializer_list>
+#include <stdexcept>
 
 glm::mat4
 flat_orientation(const glm::vec3 & diff)
@@ -57,3 +59,29 @@ Arc::Arc(const glm::vec3 & centre3, const glm::vec3 & e0p, const glm::vec3 & e1p
 	}())
 {
 }
+
+std::pair<glm::vec2, bool>
+find_arc_centre(glm::vec2 as, float entrys, glm::vec2 bs, float entrye)
+{
+	if (as == bs) {
+		return {as, false};
+	}
+	for (const auto lr : {1.F, -1.F}) { // left or right turn (maybe possible with removal of positve check below)
+		const auto perps = entrys + (half_pi * lr);
+		const auto perpe = entrye - (half_pi * lr);
+		const glm::vec2 ad {std::sin(perps), std::cos(perps)};
+		const glm::vec2 bd {std::sin(perpe), std::cos(perpe)};
+
+		const auto dx = bs.x - as.x;
+		const auto dy = bs.y - as.y;
+		const auto det = bd.x * ad.y - bd.y * ad.x;
+		if (det != 0) { // near parallel line will yield noisy results
+			const auto u = (dy * bd.x - dx * bd.y) / det;
+			const auto v = (dy * ad.x - dx * ad.y) / det;
+			if (u >= 0 && v >= 0) {
+				return {as + ad * u, lr < 0};
+			}
+		}
+	}
+	throw std::runtime_error("no intersection");
+}
diff --git a/utility/maths.h b/utility/maths.h
index f2114ef..996708f 100644
--- a/utility/maths.h
+++ b/utility/maths.h
@@ -47,4 +47,6 @@ arc_length(const Arc & arc)
 
 float normalize(float ang);
 
+std::pair<glm::vec2, bool> find_arc_centre(glm::vec2 start, float entrys, glm::vec2 end, float entrye);
+
 #endif