diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 7418ba4ef1..ca82ddd5e8 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -41,7 +41,7 @@ #include "rotation.h" __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix) +get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix) { float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; @@ -199,5 +199,10 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) y = -y; return; } + case ROTATION_PITCH_90_YAW_180: { + tmp = x; x = z; z = tmp; + y = -y; + return; + } } } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index ef0f41c3a8..8128551e51 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -76,6 +76,7 @@ enum Rotation { ROTATION_PITCH_270 = 25, ROTATION_ROLL_270_YAW_270 = 26, ROTATION_ROLL_180_PITCH_270 = 27, + ROTATION_PITCH_90_YAW_180 = 28, ROTATION_MAX }; @@ -113,14 +114,15 @@ const rot_lookup_t rot_lookup[] = { { 0, 90, 0 }, { 0, 270, 0 }, {270, 0, 270 }, - {180, 270, 0 } + {180, 270, 0 }, + { 0, 90, 180 } }; /** * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); /**