diff options
| -rw-r--r-- | game/network/network.cpp | 50 | ||||
| -rw-r--r-- | lib/maths.h | 19 |
2 files changed, 47 insertions, 22 deletions
diff --git a/game/network/network.cpp b/game/network/network.cpp index 23d39af..403975c 100644 --- a/game/network/network.cpp +++ b/game/network/network.cpp @@ -112,27 +112,33 @@ Network::genCurveDef(const GlobalPosition3D & start, const GlobalPosition3D & en std::pair<GenCurveDef, GenCurveDef> Network::genCurveDef(const GlobalPosition3D & start, const GlobalPosition3D & end, float startDir, float endDir) { - startDir += pi; - endDir += pi; - const auto flatStart {start.xy()}, flatEnd {end.xy()}; - auto midheight = [&](auto mid) { - const auto startToMid = ::distance<2>(flatStart, mid); - const auto endToMid = ::distance<2>(flatEnd, mid); - return start.z + GlobalDistance(RelativeDistance(end.z - start.z) * (startToMid / (startToMid + endToMid))); - }; - const auto radii = find_arcs_radius(flatStart, startDir, flatEnd, endDir); - if (radii.first < radii.second) { - const auto radius = radii.first; - const auto centre1 = flatStart + (sincos(startDir + half_pi) * radius); - const auto centre2 = flatEnd + (sincos(endDir + half_pi) * radius); - const auto mid = (centre1 + centre2) / 2; - const auto midh = mid || midheight(mid); - return {{start, midh, centre1}, {end, midh, centre2}}; + // Based on formula/code from https://www.ryanjuckett.com/biarc-interpolation/ + const auto startVec = -sincos(startDir); + const auto endVec = sincos(endDir); + const auto diff = difference(end, start); + const auto diffDotStartVec = glm::dot(diff.xy(), startVec); + const auto endsVecTotal = (startVec + endVec); + const auto tMagSqr = vectorMagSquared(endsVecTotal); + const auto equalTangents = isWithinLimit(tMagSqr, 4.0F); + const auto perpT1 = isWithinLimit(diffDotStartVec, 0.0F); + + if (equalTangents && perpT1) { + const auto joint = start + (diff * 0.5F); + return {genCurveDef(start, joint, startDir), genCurveDef(end, joint, endDir)}; } - const auto radius = radii.second; - const auto centre1 = flatStart + (sincos(startDir - half_pi) * radius); - const auto centre2 = flatEnd + (sincos(endDir - half_pi) * radius); - const auto mid = (centre1 + centre2) / 2; - const auto midh = mid || midheight(mid); - return {{midh, start, centre1}, {midh, end, centre2}}; + + const auto vDotT = glm::dot(diff.xy(), endsVecTotal); + const auto extLen1 = [&]() { + const auto vMagSqr = vectorMagSquared(diff); + if (equalTangents) { + return vMagSqr / (4 * diffDotStartVec); + } + const auto denominator = 2.F - (2.F * glm::dot(startVec, endVec)); + const auto discriminant = sq(vDotT) + (denominator * vMagSqr); + return (std::sqrt(discriminant) - vDotT) / denominator; + }(); + + const auto joint = (start + end + ((difference(startVec, endVec) * extLen1) || 0.F)) / 2; + + return {genCurveDef(start, joint, startDir), genCurveDef(end, joint, endDir)}; } diff --git a/lib/maths.h b/lib/maths.h index 68b45da..2999d01 100644 --- a/lib/maths.h +++ b/lib/maths.h @@ -8,6 +8,7 @@ #include <glm/gtc/constants.hpp> #include <numeric> #include <optional> +#include <ranges> #include <stdexcept> #include <utility> @@ -288,6 +289,15 @@ sq(T value) return value * value; } +template<glm::length_t L, typename T, glm::qualifier Q> +auto +vectorMagSquared(const glm::vec<L, T, Q> & val) +{ + return std::ranges::fold_left(std::views::iota(0, L), T {}, [&val](auto total, auto axis) { + return total + sq(val[axis]); + }); +} + template<glm::qualifier Q = glm::defaultp> constexpr glm::vec<3, int64_t, Q> crossProduct(const glm::vec<3, int64_t, Q> & valueA, const glm::vec<3, int64_t, Q> & valueB) @@ -403,6 +413,15 @@ linesIntersectAt(const glm::vec<2, T, Q> Aabs, const glm::vec<2, T, Q> Babs, con return Aabs + CVec {(b1 * c2) / -determinant, (a1 * c2) / determinant}; } +template<std::floating_point T> constexpr auto EPSILON = 0.0001F; + +template<std::floating_point T> +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, Rotation2D startDir, glm::vec<2, T, Q> end, Rotation2D endDir) |