libs/base/src/eu/base/axisangle.cc
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | #include "eu/base/axisangle.h" | ||
| 2 | |||
| 3 | #include <algorithm> | ||
| 4 | |||
| 5 | #include "eu/assert/assert.h" | ||
| 6 | |||
| 7 | #include "eu/base/quat.h" | ||
| 8 | |||
| 9 | namespace eu | ||
| 10 | { | ||
| 11 | 1606 | AA::AA(const n3 &ax, const An &ang) | |
| 12 | 1606 | : axis(ax), angle(ang) | |
| 13 | { | ||
| 14 | 1606 | ASSERT(ax.is_valid()); | |
| 15 | 1606 | } | |
| 16 | |||
| 17 | [[nodiscard]] AA | ||
| 18 | 4 | AA::from(const Q& q) | |
| 19 | { | ||
| 20 | 4 | const float cos_a = std::ranges::clamp(q.w , -1.0f, 1.0f); // todo(Gustav): is this needed? | |
| 21 | 4 | const auto angle = acos(cos_a) * 2; | |
| 22 | 4 | const auto sin_a = clamp_zero(std::sqrt(1.0f - cos_a * cos_a), 1, 0.0005f); | |
| 23 | // todo(Gustav): do we need to normalize here? | ||
| 24 | 4 | const auto axis = (q.get_vec_part() / sin_a).get_normalized(); | |
| 25 | 4 | if (!axis) | |
| 26 | { | ||
| 27 | // a "zero" rotation so any axis works | ||
| 28 | ✗ | return rha(kk::up, no_rotation); | |
| 29 | } | ||
| 30 | 4 | return rha(*axis, angle); | |
| 31 | } | ||
| 32 | |||
| 33 | [[nodiscard]] Ypr | ||
| 34 | ✗ | Ypr::from(const Q& q) | |
| 35 | { | ||
| 36 | // Protect against small floating-point errors. | ||
| 37 | ✗ | const auto sin_pitch = std::clamp( | |
| 38 | ✗ | 2.0f * (q.y * q.z - q.w * q.x), | |
| 39 | ✗ | -1.0f, | |
| 40 | ✗ | 1.0f | |
| 41 | ✗ | ); | |
| 42 | |||
| 43 | return | ||
| 44 | { | ||
| 45 | ✗ | .yaw = An::from_radians(std::atan2( | |
| 46 | ✗ | -2.0f * (q.w * q.y + q.x * q.z), | |
| 47 | ✗ | 1.0f - 2.0f * (q.x * q.x + q.y * q.y) | |
| 48 | )), | ||
| 49 | ✗ | .pitch = An::from_radians(std::asin(sin_pitch)), | |
| 50 | ✗ | .roll = An::from_radians(std::atan2( | |
| 51 | ✗ | 2.0f * (q.w * q.z + q.x * q.y), | |
| 52 | ✗ | 1.0f - 2.0f * (q.x * q.x + q.z * q.z) | |
| 53 | )) | ||
| 54 | ✗ | }; | |
| 55 | } | ||
| 56 | |||
| 57 | AA | ||
| 58 | 65 | rha(const n3 &axis, const An &angle) | |
| 59 | { | ||
| 60 | 65 | ASSERT(axis.is_valid()); | |
| 61 | 65 | return {axis, An::from_radians(angle.as_radians())}; | |
| 62 | } | ||
| 63 | |||
| 64 | std::string | ||
| 65 | 4 | string_from(const AA &aa) | |
| 66 | { | ||
| 67 | 4 | return fmt::format("({} {})", aa.axis, aa.angle); | |
| 68 | } | ||
| 69 | |||
| 70 | std::string | ||
| 71 | 514 | string_from(const Ypr& x) | |
| 72 | { | ||
| 73 | 514 | return fmt::format("({}, {}, {})", x.yaw, x.pitch, x.roll); | |
| 74 | } | ||
| 75 | |||
| 76 | 2 | ADD_CATCH_FORMATTER_IMPL(AA) | |
| 77 | 1 | ADD_CATCH_FORMATTER_IMPL(Ypr) | |
| 78 | } | ||
| 79 |