diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 19f9967880..3d00bf7db2 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -56,3 +56,31 @@ get_rot_quaternion(enum Rotation rot) math::radians((float)rot_lookup[rot].pitch), math::radians((float)rot_lookup[rot].yaw)}}; } + +__EXPORT void +rotate_3i(enum Rotation rot, int16_t &x, int16_t &y, int16_t &z) +{ + if (!rotate_3(rot, x, y, z)) { + // otherwise use full rotation matrix for valid rotations + if (rot < ROTATION_MAX) { + const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{(float)x, (float)y, (float)z}}; + x = math::constrain(roundf(r(0)), (float)INT16_MIN, (float)INT16_MAX); + y = math::constrain(roundf(r(1)), (float)INT16_MIN, (float)INT16_MAX); + z = math::constrain(roundf(r(2)), (float)INT16_MIN, (float)INT16_MAX); + } + } +} + +__EXPORT void +rotate_3f(enum Rotation rot, float &x, float &y, float &z) +{ + if (!rotate_3(rot, x, y, z)) { + // otherwise use full rotation matrix for valid rotations + if (rot < ROTATION_MAX) { + const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{x, y, z}}; + x = r(0); + y = r(1); + z = r(2); + } + } +} diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index b33e4a8383..350404723e 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -157,8 +157,18 @@ __EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot); */ __EXPORT matrix::Quatf get_rot_quaternion(enum Rotation rot); +/** + * rotate a 3 element int16_t vector in-place + */ +__EXPORT void rotate_3i(enum Rotation rot, int16_t &x, int16_t &y, int16_t &z); + +/** + * rotate a 3 element float vector in-place + */ +__EXPORT void rotate_3f(enum Rotation rot, float &x, float &y, float &z); + template -static constexpr bool rotate_3(enum Rotation rot, T &x, T &y, T &z) +static bool rotate_3(enum Rotation rot, T &x, T &y, T &z) { switch (rot) { case ROTATION_NONE: @@ -371,35 +381,3 @@ static constexpr bool rotate_3(enum Rotation rot, T &x, T &y, T &z) return false; } - -/** - * rotate a 3 element int16_t vector in-place - */ -__EXPORT inline void rotate_3i(enum Rotation rot, int16_t &x, int16_t &y, int16_t &z) -{ - if (!rotate_3(rot, x, y, z)) { - // otherwise use full rotation matrix for valid rotations - if (rot < ROTATION_MAX) { - const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{(float)x, (float)y, (float)z}}; - x = math::constrain(roundf(r(0)), (float)INT16_MIN, (float)INT16_MAX); - y = math::constrain(roundf(r(1)), (float)INT16_MIN, (float)INT16_MAX); - z = math::constrain(roundf(r(2)), (float)INT16_MIN, (float)INT16_MAX); - } - } -} - -/** - * rotate a 3 element float vector in-place - */ -__EXPORT inline void rotate_3f(enum Rotation rot, float &x, float &y, float &z) -{ - if (!rotate_3(rot, x, y, z)) { - // otherwise use full rotation matrix for valid rotations - if (rot < ROTATION_MAX) { - const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{x, y, z}}; - x = r(0); - y = r(1); - z = r(2); - } - } -}