Fix calculating radius and length of curved links

4 files changed, 48 insertions, 30 deletions

diff --git a/game/network/link.h b/game/network/link.h
index 725e023..4cca52c 100644
--- a/game/network/link.h
+++ b/game/network/link.h
@@ -16,7 +16,7 @@ template<typename> class Ray;
 // it has location
 class Node : public StdTypeDefs<Node> {
 public:
-	explicit Node(GlobalPosition3D p) noexcept : pos(p) {};
+	explicit Node(GlobalPosition3D p) noexcept : pos(p) { };
 	virtual ~Node() noexcept = default;
 	NO_COPY(Node);
 	NO_MOVE(Node);
@@ -38,7 +38,7 @@ public:
 		Nexts nexts {};
 	};
 
-	Link(End, End, float);
+	Link(End, End, RelativeDistance length);
 	virtual ~Link() = default;
 	NO_COPY(Link);
 	NO_MOVE(Link);
@@ -76,7 +76,7 @@ LinkStraight::~LinkStraight() = default;
 class LinkCurve : public virtual Link {
 public:
 	inline ~LinkCurve() override = 0;
-	LinkCurve(GlobalPosition3D, RelativeDistance, Arc);
+	LinkCurve(GlobalPosition3D centreBase, RelativeDistance radius, Arc);
 	NO_COPY(LinkCurve);
 	NO_MOVE(LinkCurve);
 
diff --git a/game/network/rail.cpp b/game/network/rail.cpp
index 6f04070..69422aa 100644
--- a/game/network/rail.cpp
+++ b/game/network/rail.cpp
@@ -108,16 +108,17 @@ RailLinkStraight::RailLinkStraight(
 {
 }
 
-RailLinkCurve::RailLinkCurve(NetworkLinkHolder<RailLinkCurve> & instances, const Node::Ptr & a, const Node::Ptr & b,
-		GlobalPosition2D c) : RailLinkCurve(instances, a, b, c || a->pos.z, {c, a->pos, b->pos})
+RailLinkCurve::RailLinkCurve(
+		NetworkLinkHolder<RailLinkCurve> & instances, const Node::Ptr & a, const Node::Ptr & b, GlobalPosition2D c) :
+	RailLinkCurve(instances, a, b, c || a->pos.z, glm::length(difference(a->pos.xy(), c)), {c, a->pos, b->pos})
 {
 }
 
 RailLinkCurve::RailLinkCurve(NetworkLinkHolder<RailLinkCurve> & instances, const Node::Ptr & a, const Node::Ptr & b,
-		GlobalPosition3D c, const Arc arc) :
+		GlobalPosition3D c, RelativeDistance radius, const Arc arc) :
 	Link({a, normalize(arc.first + half_pi)}, {b, normalize(arc.second - half_pi)},
-			glm::length(RelativePosition3D(a->pos - c)) * arc.length()),
-	LinkCurve {c, glm::length(RelativePosition3D(ends[0].node->pos - c)), arc},
+			glm::length(RelativePosition2D {radius * arc.length(), difference(a->pos, b->pos).z})),
+	LinkCurve {c, radius, arc},
 	instance {instances.vertices.acquire(ends[0].node->pos, ends[1].node->pos, c, round_sleepers(length / 2000.F),
 			half_pi - arc.first, half_pi - arc.second, radius)}
 {
diff --git a/game/network/rail.h b/game/network/rail.h
index c8effef..0aae718 100644
--- a/game/network/rail.h
+++ b/game/network/rail.h
@@ -62,8 +62,8 @@ public:
 	};
 
 private:
-	RailLinkCurve(
-			NetworkLinkHolder<RailLinkCurve> &, const Node::Ptr &, const Node::Ptr &, GlobalPosition3D, const Arc);
+	RailLinkCurve(NetworkLinkHolder<RailLinkCurve> &, const Node::Ptr &, const Node::Ptr &, GlobalPosition3D centreBase,
+			RelativeDistance radius, Arc);
 	InstanceVertices<Vertex>::InstanceProxy instance;
 };
 
diff --git a/test/test-network.cpp b/test/test-network.cpp
index 59eebae..ce25429 100644
--- a/test/test-network.cpp
+++ b/test/test-network.cpp
@@ -47,8 +47,8 @@ struct TestLinkS : public TestLink, public LinkStraight {
 	}
 };
 
-constexpr GlobalPosition3D p000 {0, 0, 0}, p100 {10000, 0, 0}, p200 {20000, 0, 0}, p300 {30000, 0, 0};
-constexpr GlobalPosition3D p110 {10000, 10000, 0};
+constexpr GlobalPosition3D p000 {0, 0, 500}, p100 {10500, 0, 1000}, p200 {20100, 0, 2000}, p300 {30700, 0, 3000};
+constexpr GlobalPosition3D p110 {10300, 10400, 4000};
 
 template<> NetworkLinkHolder<TestLinkS>::NetworkLinkHolder() = default;
 
@@ -208,6 +208,14 @@ BOOST_AUTO_TEST_CASE(routeTo_downStream_3to300)
 
 BOOST_AUTO_TEST_SUITE_END()
 
+namespace std {
+	std::ostream &
+	operator<<(std::ostream & s, const Link::End & e)
+	{
+		return s << std::format("End[dir: {}, loc: ({}, {}, {})]", e.dir, e.node->pos.x, e.node->pos.y, e.node->pos.z);
+	}
+}
+
 BOOST_FIXTURE_TEST_CASE(test_rail_network, RailLinks)
 {
 	//       0        1        2
@@ -221,7 +229,7 @@ BOOST_FIXTURE_TEST_CASE(test_rail_network, RailLinks)
 	//    --------
 	auto l0 = addLinksBetween(p000, p100);
 	BOOST_CHECK(dynamic_cast<RailLinkStraight *>(l0.get()));
-	BOOST_CHECK_EQUAL(l0->length, 10000);
+	BOOST_CHECK_EQUAL(l0->length, glm::length(difference(p000, p100)));
 	BOOST_CHECK_CLOSE(l0->ends[0].dir, half_pi, 0.1F);
 	BOOST_CHECK_CLOSE(l0->ends[1].dir, -half_pi, 0.1F);
 	BOOST_CHECK(l0->ends[0].nexts.empty());
@@ -229,7 +237,7 @@ BOOST_FIXTURE_TEST_CASE(test_rail_network, RailLinks)
 
 	auto l1 = addLinksBetween(p200, p100);
 	BOOST_CHECK(dynamic_cast<RailLinkStraight *>(l1.get()));
-	BOOST_CHECK_EQUAL(l1->length, 10000);
+	BOOST_CHECK_EQUAL(l1->length, glm::length(difference(p200, p100)));
 	BOOST_CHECK_CLOSE(l1->ends[0].dir, half_pi, 0.1F);
 	BOOST_CHECK_CLOSE(l1->ends[1].dir, -half_pi, 0.1F);
 	BOOST_CHECK(l0->ends[0].nexts.empty());
@@ -241,7 +249,7 @@ BOOST_FIXTURE_TEST_CASE(test_rail_network, RailLinks)
 
 	auto l2 = addLinksBetween(p200, p300);
 	BOOST_CHECK(dynamic_cast<RailLinkStraight *>(l2.get()));
-	BOOST_CHECK_EQUAL(l2->length, 10000);
+	BOOST_CHECK_EQUAL(l2->length, glm::length(difference(p200, p300)));
 	BOOST_CHECK_CLOSE(l2->ends[0].dir, half_pi, 0.1F);
 	BOOST_CHECK_CLOSE(l2->ends[1].dir, -half_pi, 0.1F);
 	BOOST_CHECK(l0->ends[0].nexts.empty());
@@ -251,19 +259,28 @@ BOOST_FIXTURE_TEST_CASE(test_rail_network, RailLinks)
 	BOOST_CHECK_EQUAL(l2->ends[0].nexts.at(0).second, 1);
 	BOOST_CHECK(l2->ends[1].nexts.empty());
 
-	auto l3 = addLinksBetween(p000, p110);
-	BOOST_CHECK(dynamic_cast<RailLinkCurve *>(l3.get()));
-	BOOST_CHECK_CLOSE(l3->length, (pi + half_pi) * 10000.F, 0.1F);
-	BOOST_CHECK_CLOSE(l3->ends[0].dir, -half_pi, 0.1F);
-	BOOST_CHECK_CLOSE(l3->ends[1].dir, 0, 0.1F);
-	BOOST_CHECK_EQUAL(l0->ends[0].nexts.at(0).first.lock(), l3);
-	BOOST_CHECK_EQUAL(l0->ends[0].nexts.at(0).second, 0);
-	BOOST_CHECK_EQUAL(l3->ends[0].nexts.at(0).first.lock(), l0);
-	BOOST_CHECK_EQUAL(l3->ends[0].nexts.at(0).second, 0);
-	BOOST_CHECK(l3->ends[1].nexts.empty());
-
-	auto l4 = addLinksBetween(p110, p300);
-	BOOST_CHECK_CLOSE(l4->length, 30400.F, 0.1F);
-	BOOST_CHECK_BETWEEN(l4->ends[0].dir, .23F, .24F);
-	BOOST_CHECK_CLOSE(l4->ends[1].dir, half_pi, 0.1F);
+	BOOST_CHECK_IF(l3, addLinksBetween(p000, p110)) {
+		BOOST_CHECK_IF(l3c, dynamic_cast<RailLinkCurve *>(l3.get())) {
+			BOOST_CHECK_CLOSE(l3c->radius, 10'300.F, 0.1F);
+			BOOST_CHECK_CLOSE(l3c->arc.length(), pi + half_pi, 0.5F);
+			BOOST_CHECK_CLOSE(l3->length, 48'563.F, 0.1F);
+			BOOST_CHECK_CLOSE(l3->ends[0].dir, -half_pi, 0.5F);
+			BOOST_CHECK_CLOSE(l3->ends[1].dir, -0.0097F, 0.5F);
+			BOOST_CHECK_EQUAL(l0->ends[0].nexts.at(0).first.lock(), l3);
+			BOOST_CHECK_EQUAL(l0->ends[0].nexts.at(0).second, 0);
+			BOOST_CHECK_EQUAL(l3->ends[0].nexts.at(0).first.lock(), l0);
+			BOOST_CHECK_EQUAL(l3->ends[0].nexts.at(0).second, 0);
+			BOOST_CHECK(l3->ends[1].nexts.empty());
+		}
+	}
+
+	BOOST_CHECK_IF(l4, addLinksBetween(p110, p300)) {
+		BOOST_CHECK_IF(l4c, dynamic_cast<RailLinkCurve *>(l4.get())) {
+			BOOST_CHECK_CLOSE(l4c->radius, 6950.F, 0.1F);
+			BOOST_CHECK_CLOSE(l4c->arc.length(), 4.456F, 0.1F);
+			BOOST_CHECK_CLOSE(l4->length, 30'981.F, 0.1F);
+			BOOST_CHECK_BETWEEN(l4->ends[0].dir, .25F, .26F);
+			BOOST_CHECK_CLOSE(l4->ends[1].dir, half_pi, 0.1F);
+		}
+	}
 }