diff options
Diffstat (limited to 'lib/maths.cpp')
| -rw-r--r-- | lib/maths.cpp | 46 |
1 files changed, 24 insertions, 22 deletions
diff --git a/lib/maths.cpp b/lib/maths.cpp index b8dbd34..5430ef6 100644 --- a/lib/maths.cpp +++ b/lib/maths.cpp @@ -6,7 +6,7 @@ #include <stdexcept> glm::mat4 -flat_orientation(const glm::vec3 & diff) +flat_orientation(const Direction3D & diff) { static const auto oneeighty {glm::rotate(pi, up)}; const auto flatdiff {glm::normalize(!!diff)}; @@ -16,17 +16,17 @@ flat_orientation(const glm::vec3 & diff) } // Helper to lookup into a matrix given an xy vector coordinate -template<typename M> +template<typename M, typename I> inline auto & -operator^(M & m, glm::ivec2 xy) +operator^(M & m, glm::vec<2, I> xy) { return m[xy.x][xy.y]; } // Create a matrix for the angle, given the targets into the matrix -template<typename M> +template<typename M, typename I> inline auto -rotation(typename M::value_type a, glm::ivec2 c1, glm::ivec2 s1, glm::ivec2 c2, glm::ivec2 ms2) +rotation(typename M::value_type a, glm::vec<2, I> c1, glm::vec<2, I> s1, glm::vec<2, I> c2, glm::vec<2, I> ms2) { M m(1); sincosf(a, m ^ s1, m ^ c1); @@ -39,51 +39,51 @@ rotation(typename M::value_type a, glm::ivec2 c1, glm::ivec2 s1, glm::ivec2 c2, glm::mat2 rotate_flat(float a) { - return rotation<glm::mat2>(a, {0, 0}, {0, 1}, {1, 1}, {1, 0}); + return rotation<glm::mat2, glm::length_t>(a, {0, 0}, {0, 1}, {1, 1}, {1, 0}); } // Create a yaw transformation matrix glm::mat4 rotate_yaw(float a) { - return rotation<glm::mat4>(a, {0, 0}, {1, 0}, {1, 1}, {0, 1}); + return rotation<glm::mat4, glm::length_t>(a, {0, 0}, {1, 0}, {1, 1}, {0, 1}); } // Create a roll transformation matrix glm::mat4 rotate_roll(float a) { - return rotation<glm::mat4>(a, {0, 0}, {2, 0}, {2, 2}, {0, 2}); + return rotation<glm::mat4, glm::length_t>(a, {0, 0}, {2, 0}, {2, 2}, {0, 2}); } // Create a pitch transformation matrix glm::mat4 rotate_pitch(float a) { - return rotation<glm::mat4>(a, {1, 1}, {1, 2}, {2, 2}, {2, 1}); + return rotation<glm::mat4, glm::length_t>(a, {1, 1}, {1, 2}, {2, 2}, {2, 1}); } // Create a combined yaw, pitch, roll transformation matrix glm::mat4 -rotate_ypr(glm::vec3 a) +rotate_ypr(Rotation3D a) { return rotate_yaw(a.y) * rotate_pitch(a.x) * rotate_roll(a.z); } glm::mat4 -rotate_yp(glm::vec2 a) +rotate_yp(Rotation2D a) { return rotate_yaw(a.y) * rotate_pitch(a.x); } float -vector_yaw(const glm::vec3 & diff) +vector_yaw(const Direction3D & diff) { return std::atan2(diff.x, diff.y); } float -vector_pitch(const glm::vec3 & diff) +vector_pitch(const Direction3D & diff) { return std::atan(diff.z); } @@ -106,7 +106,7 @@ normalize(float ang) return ang; } -Arc::Arc(const glm::vec3 & centre3, const glm::vec3 & e0p, const glm::vec3 & e1p) : +Arc::Arc(const Position3D & centre3, const Position3D & e0p, const Position3D & e1p) : Arc([&]() -> Arc { const auto diffa = e0p - centre3; const auto diffb = e1p - centre3; @@ -120,8 +120,8 @@ 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) +std::pair<Position2D, bool> +find_arc_centre(Position2D as, float entrys, Position2D bs, float entrye) { if (as == bs) { return {as, false}; @@ -129,8 +129,8 @@ find_arc_centre(glm::vec2 as, float entrys, glm::vec2 bs, float entrye) return find_arc_centre(as, sincosf(entrys + half_pi), bs, sincosf(entrye - half_pi)); } -std::pair<glm::vec2, bool> -find_arc_centre(glm::vec2 as, glm::vec2 ad, glm::vec2 bs, glm::vec2 bd) +std::pair<Position2D, bool> +find_arc_centre(Position2D as, Position2D ad, Position2D bs, Position2D bd) { const auto det = bd.x * ad.y - bd.y * ad.x; if (det != 0) { // near parallel line will yield noisy results @@ -142,7 +142,7 @@ find_arc_centre(glm::vec2 as, glm::vec2 ad, glm::vec2 bs, glm::vec2 bd) } std::pair<float, float> -find_arcs_radius(glm::vec2 start, float entrys, glm::vec2 end, float entrye) +find_arcs_radius(Position2D start, float entrys, Position2D end, float entrye) { const auto getrad = [&](float leftOrRight) { return find_arcs_radius(start, sincosf(entrys + leftOrRight), end, sincosf(entrye + leftOrRight)); @@ -151,7 +151,7 @@ find_arcs_radius(glm::vec2 start, float entrys, glm::vec2 end, float entrye) } float -find_arcs_radius(glm::vec2 start, glm::vec2 ad, glm::vec2 end, glm::vec2 bd) +find_arcs_radius(Position2D start, Position2D ad, Position2D end, Position2D bd) { // Short name functions for big forula auto sqrt = [](float v) { @@ -177,12 +177,14 @@ find_arcs_radius(glm::vec2 start, glm::vec2 ad, glm::vec2 end, glm::vec2 bd) / (2 * (sq(X) - 2 * X * Z + sq(Z) + sq(Y) - 2 * Y * W + sq(W) - 4)); } -float operator"" _mph(const long double v) +float +operator"" _mph(const long double v) { return static_cast<float>(mph_to_ms(v)); } -float operator"" _kph(const long double v) +float +operator"" _kph(const long double v) { return static_cast<float>(kph_to_ms(v)); } |