From 2b8e981a7d7fb9e7b381df345d9b1a65b6ed4629 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:51:47 +0200 Subject: [PATCH] conversion lib: Update code style --- src/lib/conversion/rotation.cpp | 329 +++++++++++++++++--------------- src/lib/conversion/rotation.h | 2 +- 2 files changed, 180 insertions(+), 151 deletions(-) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index ca82ddd5e8..f56513e209 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -55,154 +55,183 @@ get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix) __EXPORT void rotate_3f(enum Rotation rot, float &x, float &y, float &z) { - float tmp; - switch (rot) { - case ROTATION_NONE: - case ROTATION_MAX: - return; - case ROTATION_YAW_45: { - tmp = HALF_SQRT_2*(x - y); - y = HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_YAW_90: { - tmp = x; x = -y; y = tmp; - return; - } - case ROTATION_YAW_135: { - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; - return; - } - case ROTATION_YAW_180: - x = -x; y = -y; - return; - case ROTATION_YAW_225: { - tmp = HALF_SQRT_2*(y - x); - y = -HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_YAW_270: { - tmp = x; x = y; y = -tmp; - return; - } - case ROTATION_YAW_315: { - tmp = HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(y - x); - x = tmp; - return; - } - case ROTATION_ROLL_180: { - y = -y; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_45: { - tmp = HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_90: { - tmp = x; x = y; y = tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_135: { - tmp = HALF_SQRT_2*(y - x); - y = HALF_SQRT_2*(y + x); - x = tmp; z = -z; - return; - } - case ROTATION_PITCH_180: { - x = -x; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_225: { - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(y - x); - x = tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_270: { - tmp = x; x = -y; y = -tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_315: { - tmp = HALF_SQRT_2*(x - y); - y = -HALF_SQRT_2*(x + y); - x = tmp; z = -z; - return; - } - case ROTATION_ROLL_90: { - tmp = z; z = y; y = -tmp; - return; - } - case ROTATION_ROLL_90_YAW_45: { - tmp = z; z = y; y = -tmp; - tmp = HALF_SQRT_2*(x - y); - y = HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_ROLL_90_YAW_90: { - tmp = z; z = y; y = -tmp; - tmp = x; x = -y; y = tmp; - return; - } - case ROTATION_ROLL_90_YAW_135: { - tmp = z; z = y; y = -tmp; - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; - return; - } - case ROTATION_ROLL_270: { - tmp = z; z = -y; y = tmp; - return; - } - case ROTATION_ROLL_270_YAW_45: { - tmp = z; z = -y; y = tmp; - tmp = HALF_SQRT_2*(x - y); - y = HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_ROLL_270_YAW_90: { - tmp = z; z = -y; y = tmp; - tmp = x; x = -y; y = tmp; - return; - } - case ROTATION_ROLL_270_YAW_135: { - tmp = z; z = -y; y = tmp; - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; - return; - } - case ROTATION_ROLL_270_YAW_270: { - tmp = z; z = -y; y = tmp; - tmp = x; x = y; y = -tmp; - return; - } - case ROTATION_PITCH_90: { - tmp = z; z = -x; x = tmp; - return; - } - case ROTATION_PITCH_270: { - tmp = z; z = x; x = -tmp; - return; - } - case ROTATION_ROLL_180_PITCH_270: { - tmp = z; z = x; x = tmp; - y = -y; - return; - } - case ROTATION_PITCH_90_YAW_180: { - tmp = x; x = z; z = tmp; - y = -y; - return; - } - } + float tmp; + + switch (rot) { + case ROTATION_NONE: + case ROTATION_MAX: + return; + + case ROTATION_YAW_45: { + tmp = HALF_SQRT_2 * (x - y); + y = HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_YAW_90: { + tmp = x; x = -y; y = tmp; + return; + } + + case ROTATION_YAW_135: { + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; + return; + } + + case ROTATION_YAW_180: + x = -x; y = -y; + return; + + case ROTATION_YAW_225: { + tmp = HALF_SQRT_2 * (y - x); + y = -HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_YAW_270: { + tmp = x; x = y; y = -tmp; + return; + } + + case ROTATION_YAW_315: { + tmp = HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (y - x); + x = tmp; + return; + } + + case ROTATION_ROLL_180: { + y = -y; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_45: { + tmp = HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_90: { + tmp = x; x = y; y = tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_135: { + tmp = HALF_SQRT_2 * (y - x); + y = HALF_SQRT_2 * (y + x); + x = tmp; z = -z; + return; + } + + case ROTATION_PITCH_180: { + x = -x; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_225: { + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (y - x); + x = tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_270: { + tmp = x; x = -y; y = -tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_315: { + tmp = HALF_SQRT_2 * (x - y); + y = -HALF_SQRT_2 * (x + y); + x = tmp; z = -z; + return; + } + + case ROTATION_ROLL_90: { + tmp = z; z = y; y = -tmp; + return; + } + + case ROTATION_ROLL_90_YAW_45: { + tmp = z; z = y; y = -tmp; + tmp = HALF_SQRT_2 * (x - y); + y = HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_ROLL_90_YAW_90: { + tmp = z; z = y; y = -tmp; + tmp = x; x = -y; y = tmp; + return; + } + + case ROTATION_ROLL_90_YAW_135: { + tmp = z; z = y; y = -tmp; + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; + return; + } + + case ROTATION_ROLL_270: { + tmp = z; z = -y; y = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_45: { + tmp = z; z = -y; y = tmp; + tmp = HALF_SQRT_2 * (x - y); + y = HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_90: { + tmp = z; z = -y; y = tmp; + tmp = x; x = -y; y = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_135: { + tmp = z; z = -y; y = tmp; + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_270: { + tmp = z; z = -y; y = tmp; + tmp = x; x = y; y = -tmp; + return; + } + + case ROTATION_PITCH_90: { + tmp = z; z = -x; x = tmp; + return; + } + + case ROTATION_PITCH_270: { + tmp = z; z = x; x = -tmp; + return; + } + + case ROTATION_ROLL_180_PITCH_270: { + tmp = z; z = x; x = tmp; + 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 8128551e51..280b9cad0c 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -122,7 +122,7 @@ const rot_lookup_t rot_lookup[] = { * 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); /**