Fix calculating radius and length of curved links

1 files changed, 37 insertions, 20 deletions

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);
+		}
+	}
 }