diff options
-rw-r--r-- | game/network/network.cpp | 54 | ||||
-rw-r--r-- | lib/maths.cpp | 9 | ||||
-rw-r--r-- | lib/maths.h | 64 | ||||
-rw-r--r-- | test/test-maths.cpp | 6 | ||||
-rw-r--r-- | test/test-network.cpp | 115 |
5 files changed, 164 insertions, 84 deletions
diff --git a/game/network/network.cpp b/game/network/network.cpp index 70c4e21..ae4865a 100644 --- a/game/network/network.cpp +++ b/game/network/network.cpp @@ -179,10 +179,13 @@ Network::genDef(const GlobalPosition3D & start, const GlobalPosition3D & end, An const auto flatStart = start.xy(), flatEnd = end.xy(); const auto centre = find_arc_centre(flatStart, dir, flatEnd); - if (centre.second) { // right hand arc + if (centre.second > 0.1F) { return {GenCurveDef {end, start, centre.first}}; } - return {GenCurveDef {start, end, centre.first}}; + if (centre.second < -0.1F) { + return {GenCurveDef {start, end, centre.first}}; + } + return {GenStraightDef {start, end}}; } GenLinksDef @@ -219,13 +222,38 @@ Network::genDef(const GlobalPosition3D & start, const GlobalPosition3D & end, An return genDef(start, joint, startDir) + genDef(end, joint, endDir); } +namespace { + struct MergeEq { + bool + operator()(GenCurveDef & lhs, const GenCurveDef & rhs) const + { + if (distance(std::get<2>(lhs), std::get<2>(rhs)) < 100.F) { // LHS.centre near RHS.centre + if (std::get<1>(lhs) == std::get<0>(rhs)) { // LHS.end == RHS.start + std::get<1>(lhs) = std::get<1>(rhs); + } + else if (std::get<0>(lhs) == std::get<1>(rhs)) { // LHS.start == RHS.end + std::get<0>(lhs) = std::get<0>(rhs); + } + else { + return false; + } + std::get<2>(lhs) = midpoint(std::get<2>(lhs), std::get<2>(rhs)); + return true; + } + return false; + } + + bool + operator()(const auto &, const auto &) const + { + return false; + } + }; +} + Link::Collection Network::create(const GeoData * geoData, const CreationDefinition & def) { - // TODO - // Where to make a straight to join because angles align? - // Where to drop part of S curve pair if a single curve works? - const auto linkDefs = [&def]() -> GenLinksDef { if (!def.fromEnd.direction && !def.toEnd.direction) { // No specific directions at either end, straight link @@ -251,8 +279,20 @@ Network::create(const GeoData * geoData, const CreationDefinition & def) def); }); }; + // Merge adjacent pairs where possible + auto linkDefsGen = geoData ? splitDefs() : linkDefs(); + if (!linkDefsGen.empty()) { + for (auto lhsIter = linkDefsGen.begin(), rhsIter = lhsIter + 1; rhsIter != linkDefsGen.end();) { + if (std::visit(MergeEq {}, *lhsIter, *rhsIter)) { + rhsIter = linkDefsGen.erase(rhsIter); + } + else { + lhsIter = rhsIter++; + } + } + } Link::Collection links; - std::ranges::transform(geoData ? splitDefs() : linkDefs(), std::back_inserter(links), [this](const auto & def) { + std::ranges::transform(linkDefsGen, std::back_inserter(links), [this](const auto & def) { return std::visit( [this](const auto & typedDef) { return this->create(typedDef); diff --git a/lib/maths.cpp b/lib/maths.cpp index 000cea7..7dd313e 100644 --- a/lib/maths.cpp +++ b/lib/maths.cpp @@ -1,5 +1,6 @@ #include "maths.h" #include <cmath> +#include <functional> #include <glm/glm.hpp> #include <glm/gtx/rotate_vector.hpp> #include <glm/gtx/transform.hpp> @@ -32,6 +33,7 @@ static_assert(pow(3, 1) == 3); static_assert(pow(3, 2) == 9); static_assert(pow(pi, 3) == 31.006278991699219F); +#ifndef __clang__ static_assert(!linesIntersectAt<int>({0, 10}, {10, 10}, {10, 0}, {0, 0}).has_value()); static_assert(*linesIntersectAt<int>({0, 0}, {10, 10}, {10, 0}, {0, 10}) == GlobalPosition2D {5, 5}); static_assert(*linesIntersectAt<int>({300'000'000, 400'000'00}, {300'010'000, 400'010'00}, {310'010'000, 410'000'00}, @@ -43,6 +45,13 @@ constexpr auto EAST2D = RelativePosition2D(east); static_assert(!linesIntersectAtDirs<int>({0, 0}, NORTH2D, {10, 10}, NORTH2D).has_value()); static_assert(linesIntersectAtDirs<int>({0, 0}, NORTH2D, {10, 10}, EAST2D) == GlobalPosition2D {0, 10}); static_assert(linesIntersectAtDirs<int>({0, 0}, EAST2D, {10, 10}, NORTH2D) == GlobalPosition2D {10, 0}); + +static_assert(find_arc_centre<int>({0, 0}, -NORTH2D, {10, 10}) == std::make_pair<GlobalPosition2D>({10, 0}, 10.F)); +static_assert(find_arc_centre<int>({20, 0}, -NORTH2D, {10, 10}) == std::make_pair<GlobalPosition2D>({10, 0}, -10.F)); +static_assert(find_arc_centre<int>({10, 0}, -NORTH2D, {10, 10}).second == 0.F); +static_assert(isWithinLimit(find_arc_centre<int>({0, 0}, pi + quarter_pi, {1000, 1000}).second, 0.F, 0.1F)); +#endif + // NOLINTEND(readability-magic-numbers) float diff --git a/lib/maths.h b/lib/maths.h index 3ef12e7..09af048 100644 --- a/lib/maths.h +++ b/lib/maths.h @@ -427,70 +427,34 @@ linesIntersectAt(const glm::vec<2, T, Q> Aabs, const glm::vec<2, T, Q> Babs, con template<std::floating_point T> constexpr auto EPSILON = 0.0001F; template<std::floating_point T> -auto +[[nodiscard]] constexpr auto isWithinLimit(T lhs, T rhs, T limit = EPSILON<T>) { return std::abs(lhs - rhs) <= limit; } -template<Arithmetic T, glm::qualifier Q = glm::defaultp> -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) -{ - if (start == end) { - return {start, false}; - } - return find_arc_centre(start, sincos(entrys + half_pi), end, sincos(entrye - half_pi)); -} - -template<Arithmetic T, glm::qualifier Q = glm::defaultp> -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) +template<Arithmetic T, std::floating_point D, glm::qualifier Q = glm::defaultp> +constexpr std::pair<glm::vec<2, T, Q>, D> +find_arc_centre(glm::vec<2, T, Q> start, glm::vec<2, D, Q> entrys, glm::vec<2, T, Q> end) { if (start == end) { - return {start, false}; + return {start, 0}; } - const auto startNormal = vector_normal(sincos(entrys) * 10'000.F); const auto diffEnds = difference(end, start); + const auto offset = entrys.x * diffEnds.y - entrys.y * diffEnds.x; + if (offset == 0.F) { + return {start, offset}; + } const auto midEnds = start + ((end - start) / 2); - const auto diffNormal = vector_normal(diffEnds); - const auto centre = linesIntersectAt(start, start + startNormal, midEnds, midEnds + diffNormal); - return {*centre, normalize(vector_yaw(diffEnds) - entrys) < 0}; -} - -template<Arithmetic T, glm::qualifier Q = glm::defaultp> -Angle -find_arcs_radius(glm::vec<2, T, Q> start, Rotation2D ad, glm::vec<2, T, Q> end, Rotation2D bd) -{ - using std::sqrt; - - // Calculates path across both arcs along the normals... pythagorean theorem... for some known radius r - // (2r)^2 = ((m + (X*r)) - (o + (Z*r)))^2 + ((n + (Y*r)) - (p + (W*r)))^2 - // According to symbolabs.com equation tool, that solves for r to give: - // r=(-2 m X+2 X o+2 m Z-2 o Z-2 n Y+2 Y p+2 n W-2 p W-sqrt((2 m X-2 X o-2 m Z+2 o Z+2 n Y-2 Y p-2 n W+2 p W)^(2)-4 - // (X^(2)-2 X Z+Z^(2)+Y^(2)-2 Y W+W^(2)-4) (m^(2)-2 m o+o^(2)+n^(2)-2 n p+p^(2))))/(2 (X^(2)-2 X Z+Z^(2)+Y^(2)-2 Y - // W+W^(2)-4)) - // Locally simplified to work relative, removing one half of the problem and operating on relative positions. - - // These exist cos limitations of online formula rearrangement, and I'm OK with that. - const RelativePosition2D diff {end - start}; - const auto &o {diff.x}, &p {diff.y}; - const auto &X {ad.x}, &Y {ad.y}, &Z {bd.x}, &W {bd.y}; - - return (-2 * X * o + 2 * o * Z - 2 * Y * p + 2 * p * W - - sqrt(sq(2 * X * o - 2 * o * Z + 2 * Y * p - 2 * p * W) - - (4 * (sq(X) - 2 * X * Z + sq(Z) + sq(Y) - 2 * Y * W + sq(W) - 4) * (sq(o) + sq(p))))) - / (2 * (sq(X) - 2 * X * Z + sq(Z) + sq(Y) - 2 * Y * W + sq(W) - 4)); + const auto centre = linesIntersectAtDirs(start, vector_normal(entrys), midEnds, vector_normal(diffEnds)); + return {*centre, offset}; } template<Arithmetic T, glm::qualifier Q = glm::defaultp> -std::pair<Angle, Angle> -find_arcs_radius(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, Angle entrye) +constexpr std::pair<glm::vec<2, T, Q>, float> +find_arc_centre(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end) { - const auto getrad = [&](auto leftOrRight) { - return find_arcs_radius(start, sincos(entrys + leftOrRight), end, sincos(entrye + leftOrRight)); - }; - return {getrad(-half_pi), getrad(half_pi)}; + return find_arc_centre(start, sincos(entrys), end); } template<Arithmetic T> diff --git a/test/test-maths.cpp b/test/test-maths.cpp index 5ee815d..98a7b1e 100644 --- a/test/test-maths.cpp +++ b/test/test-maths.cpp @@ -151,12 +151,6 @@ BOOST_DATA_TEST_CASE(TestCreateArc, BOOST_CHECK_CLOSE(arc.second, expAngles.second, 1.F); } -BOOST_AUTO_TEST_CASE(TestFindArcsRadius) -{ - BOOST_CHECK_CLOSE( - find_arcs_radius(RelativePosition2D {10.32, 26.71}, {0.4, .92}, {2.92, 22.41}, {-0.89, -0.45}), 2.29, 1); -} - namespace { struct TestLinkStraight : public LinkStraight { explicit TestLinkStraight(glm::vec3 entryVector) : diff --git a/test/test-network.cpp b/test/test-network.cpp index 0739073..9a9ceb3 100644 --- a/test/test-network.cpp +++ b/test/test-network.cpp @@ -102,21 +102,25 @@ namespace { struct TestNetwork : public EmptyNetwork { TestNetwork() { - /* 0 1 2 - / -> p000 <-> p100 <-> p200 <-> p300 - / / / \. - / 3 5 4 - / \ / / - / --> p110 <------------- */ + /* + * 4--> p110 <---------6------- + * / 8 \. + * 3 7 5 + * \ \ / + * -> p000 <-0-> p100 <-1-> p200 <-2-> p300 */ + // 0, 1 and 2 - 3 straight links between 4 nodes add(nullptr, createChain(nullptr, std::array {P000, P100, P200, P300})); + // 3 and 4 - RH biarc from p000 to p110 add(nullptr, create(nullptr, {.fromEnd = {.position = P000, .direction = -half_pi}, .toEnd = {.position = P110, .direction = -half_pi}})); + // 5 and 6 - LH biarc from p200 to p110 add(nullptr, create(nullptr, {.fromEnd = {.position = P200, .direction = half_pi}, .toEnd = {.position = P110, .direction = half_pi}})); + // 7 and 8 - RL S biarc from p100 to p110 add(nullptr, create(nullptr, {.fromEnd = {.position = P100, .direction = -half_pi}, @@ -182,21 +186,25 @@ BOOST_AUTO_TEST_CASE(NetworkJoins) // Join 0 <-> 1 BOOST_REQUIRE_EQUAL(links[0]->ends[1].nexts.size(), 2); BOOST_CHECK_EQUAL(links[0]->ends[1].nexts[0].first.lock().get(), links[1].get()); - BOOST_CHECK_EQUAL(links[0]->ends[1].nexts[0].second, 1); + BOOST_CHECK_EQUAL(links[0]->ends[1].nexts[0].second, 0); BOOST_CHECK_EQUAL(links[0]->ends[1].nexts[1].first.lock().get(), links[7].get()); BOOST_CHECK_EQUAL(links[0]->ends[1].nexts[1].second, 1); BOOST_REQUIRE_EQUAL(links[1]->ends[0].nexts.size(), 2); - BOOST_CHECK_EQUAL(links[1]->ends[0].nexts[0].first.lock().get(), links[2].get()); - BOOST_CHECK_EQUAL(links[1]->ends[0].nexts[0].second, 0); - BOOST_CHECK_EQUAL(links[1]->ends[0].nexts[1].first.lock().get(), links[5].get()); - BOOST_CHECK_EQUAL(links[1]->ends[0].nexts[1].second, 0); + BOOST_CHECK_EQUAL(links[1]->ends[0].nexts[0].first.lock().get(), links[0].get()); + BOOST_CHECK_EQUAL(links[1]->ends[0].nexts[0].second, 1); + BOOST_CHECK_EQUAL(links[1]->ends[0].nexts[1].first.lock().get(), links[7].get()); + BOOST_CHECK_EQUAL(links[1]->ends[0].nexts[1].second, 1); // Join 1 <-> 2 BOOST_REQUIRE_EQUAL(links[1]->ends[1].nexts.size(), 2); - BOOST_CHECK_EQUAL(links[1]->ends[1].nexts[0].first.lock().get(), links[0].get()); - BOOST_CHECK_EQUAL(links[1]->ends[1].nexts[0].second, 1); + BOOST_CHECK_EQUAL(links[1]->ends[1].nexts[0].first.lock().get(), links[2].get()); + BOOST_CHECK_EQUAL(links[1]->ends[1].nexts[0].second, 0); + BOOST_CHECK_EQUAL(links[1]->ends[1].nexts[1].first.lock().get(), links[5].get()); + BOOST_CHECK_EQUAL(links[1]->ends[1].nexts[1].second, 0); BOOST_REQUIRE_EQUAL(links[2]->ends[0].nexts.size(), 2); BOOST_CHECK_EQUAL(links[2]->ends[0].nexts[0].first.lock().get(), links[1].get()); - BOOST_CHECK_EQUAL(links[2]->ends[0].nexts[0].second, 0); + BOOST_CHECK_EQUAL(links[2]->ends[0].nexts[0].second, 1); + BOOST_CHECK_EQUAL(links[2]->ends[0].nexts[1].first.lock().get(), links[5].get()); + BOOST_CHECK_EQUAL(links[2]->ends[0].nexts[1].second, 0); } BOOST_DATA_TEST_CASE(RouteToNodeNotInNetwork, INVALID_NODES, dest) @@ -302,6 +310,54 @@ BOOST_AUTO_TEST_CASE(NetworkCreateStraight) BOOST_CHECK_EQUAL(nodes.size(), 2); } +BOOST_AUTO_TEST_CASE(NetworkCreateExtendingStraight) +{ + const auto link = create(nullptr, + CreationDefinition { + .fromEnd = {.position = {0, 0, 0}, .direction = pi + quarter_pi}, + .toEnd = {.position = {1000, 1000, 0}, .direction = {}}, + }); + BOOST_REQUIRE_EQUAL(link.size(), 1); + BOOST_CHECK(links.empty()); + BOOST_CHECK(nodes.empty()); + + BOOST_CHECK_IF(straight, std::dynamic_pointer_cast<TestLinkS>(link.front())) { + BOOST_CHECK_CLOSE(straight->length, 1414, 1); + BOOST_CHECK_CLOSE_VECI(straight->ends.front().node->pos, GlobalPosition3D(0, 0, 0)); + BOOST_CHECK_CLOSE(straight->ends.front().dir, quarter_pi - pi, 1); + BOOST_CHECK_CLOSE_VECI(straight->ends.back().node->pos, GlobalPosition3D(1000, 1000, 0)); + BOOST_CHECK_CLOSE(straight->ends.back().dir, quarter_pi, 1); + } + + add(nullptr, link.front()); + BOOST_CHECK_EQUAL(links.size(), 1); + BOOST_CHECK_EQUAL(nodes.size(), 2); +} + +BOOST_AUTO_TEST_CASE(NetworkCreateExtendeeStraight) +{ + const auto link = create(nullptr, + CreationDefinition { + .fromEnd = {.position = {0, 0, 0}, .direction = {}}, + .toEnd = {.position = {1000, 1000, 0}, .direction = quarter_pi}, + }); + BOOST_REQUIRE_EQUAL(link.size(), 1); + BOOST_CHECK(links.empty()); + BOOST_CHECK(nodes.empty()); + + BOOST_CHECK_IF(straight, std::dynamic_pointer_cast<TestLinkS>(link.front())) { + BOOST_CHECK_CLOSE(straight->length, 1414, 1); + BOOST_CHECK_CLOSE_VECI(straight->ends.front().node->pos, GlobalPosition3D(1000, 1000, 0)); + BOOST_CHECK_CLOSE(straight->ends.front().dir, quarter_pi, 1); + BOOST_CHECK_CLOSE_VECI(straight->ends.back().node->pos, GlobalPosition3D(0, 0, 0)); + BOOST_CHECK_CLOSE(straight->ends.back().dir, quarter_pi - pi, 1); + } + + add(nullptr, link.front()); + BOOST_CHECK_EQUAL(links.size(), 1); + BOOST_CHECK_EQUAL(nodes.size(), 2); +} + BOOST_AUTO_TEST_CASE(NetworkCreateExtendingCurve) { const auto link = create(nullptr, @@ -396,24 +452,41 @@ BOOST_AUTO_TEST_CASE(NetworkCreateBiarcPair) BOOST_CHECK_EQUAL(link.back()->ends.back().nexts.front().first.lock(), link.front()); } -BOOST_AUTO_TEST_CASE(NetworkCreateBiarcPairEqTan) +BOOST_AUTO_TEST_CASE(NetworkCreateBiarcPairEqTanLH) { - // This could be achieved with a single curve, but not there yet + // Creates a biarc pair and then merges them into a single curve const auto link = create(nullptr, CreationDefinition { - .fromEnd = {.position = {0, 0, 0}, .direction = 0}, - .toEnd = {.position = {1000, 0, 0}, .direction = 0}, + .fromEnd = {.position = {1000, 0, 0}, .direction = 0}, + .toEnd = {.position = {0, 0, 0}, .direction = 0}, }); - BOOST_REQUIRE_EQUAL(link.size(), 2); + BOOST_REQUIRE_EQUAL(link.size(), 1); BOOST_CHECK(links.empty()); BOOST_CHECK(nodes.empty()); BOOST_CHECK_IF(firstCurve, std::dynamic_pointer_cast<TestLinkC>(link.front())) { + BOOST_CHECK_CLOSE_VECI(firstCurve->ends.front().node->pos, GlobalPosition3D(1000, 0, 0)); + BOOST_CHECK_CLOSE_VECI(firstCurve->ends.back().node->pos, GlobalPosition3D(0, 0, 0)); BOOST_CHECK_CLOSE_VECI(firstCurve->centreBase, GlobalPosition3D(500, 0, 0)); } +} - BOOST_CHECK_IF(secondCurve, std::dynamic_pointer_cast<TestLinkC>(link.back())) { - BOOST_CHECK_CLOSE_VECI(secondCurve->centreBase, GlobalPosition3D(500, 0, 0)); +BOOST_AUTO_TEST_CASE(NetworkCreateBiarcPairEqTanRH) +{ + // Creates a biarc pair and then merges them into a single curve + const auto link = create(nullptr, + CreationDefinition { + .fromEnd = {.position = {0, 0, 0}, .direction = 0}, + .toEnd = {.position = {1000, 0, 0}, .direction = 0}, + }); + BOOST_REQUIRE_EQUAL(link.size(), 1); + BOOST_CHECK(links.empty()); + BOOST_CHECK(nodes.empty()); + + BOOST_CHECK_IF(firstCurve, std::dynamic_pointer_cast<TestLinkC>(link.front())) { + BOOST_CHECK_CLOSE_VECI(firstCurve->ends.front().node->pos, GlobalPosition3D(1000, 0, 0)); + BOOST_CHECK_CLOSE_VECI(firstCurve->ends.back().node->pos, GlobalPosition3D(0, 0, 0)); + BOOST_CHECK_CLOSE_VECI(firstCurve->centreBase, GlobalPosition3D(500, 0, 0)); } } |