Browse Source

AP_Math: make duplicate rotations clear

gps-1.3.1
Iampete1 4 years ago committed by Andrew Tridgell
parent
commit
dbd95f8163
  1. 13
      libraries/AP_Math/quaternion.cpp
  2. 4
      libraries/AP_Math/rotations.h
  3. 16
      libraries/AP_Math/vector3.cpp

13
libraries/AP_Math/quaternion.cpp

@ -189,6 +189,7 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation) @@ -189,6 +189,7 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
return;
case ROTATION_ROLL_180_YAW_90:
case ROTATION_PITCH_180_YAW_270:
q1 = q4 = 0;
q2 = q3 = HALF_SQRT_2;
return;
@ -211,6 +212,7 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation) @@ -211,6 +212,7 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
return;
case ROTATION_ROLL_180_YAW_270:
case ROTATION_PITCH_180_YAW_90:
q1 = q4 = 0;
q2 = -HALF_SQRT_2;
q3 = HALF_SQRT_2;
@ -279,17 +281,6 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation) @@ -279,17 +281,6 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
q3 = -HALF_SQRT_2;
return;
case ROTATION_PITCH_180_YAW_90:
q1 = q4 = 0;
q2 = -HALF_SQRT_2;
q3 = HALF_SQRT_2;
return;
case ROTATION_PITCH_180_YAW_270:
q1 = q4 = 0;
q2 = q3 = HALF_SQRT_2;
return;
case ROTATION_ROLL_90_PITCH_90:
q1 = q2 = q3 = -0.5f;
q4 = 0.5f;

4
libraries/AP_Math/rotations.h

@ -51,8 +51,8 @@ enum Rotation : uint8_t { @@ -51,8 +51,8 @@ enum Rotation : uint8_t {
ROTATION_ROLL_270_YAW_135 = 23,
ROTATION_PITCH_90 = 24,
ROTATION_PITCH_270 = 25,
ROTATION_PITCH_180_YAW_90 = 26,
ROTATION_PITCH_180_YAW_270 = 27,
ROTATION_PITCH_180_YAW_90 = 26, // same as ROTATION_ROLL_180_YAW_270
ROTATION_PITCH_180_YAW_270 = 27, // same as ROTATION_ROLL_180_YAW_90
ROTATION_ROLL_90_PITCH_90 = 28,
ROTATION_ROLL_180_PITCH_90 = 29,
ROTATION_ROLL_270_PITCH_90 = 30,

16
libraries/AP_Math/vector3.cpp

@ -75,7 +75,8 @@ void Vector3<T>::rotate(enum Rotation rotation) @@ -75,7 +75,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
x = tmp; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_90: {
case ROTATION_ROLL_180_YAW_90:
case ROTATION_PITCH_180_YAW_270: {
tmp = x; x = y; y = tmp; z = -z;
return;
}
@ -95,7 +96,8 @@ void Vector3<T>::rotate(enum Rotation rotation) @@ -95,7 +96,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
x = tmp; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_270: {
case ROTATION_ROLL_180_YAW_270:
case ROTATION_PITCH_180_YAW_90: {
tmp = x; x = -y; y = -tmp; z = -z;
return;
}
@ -159,16 +161,6 @@ void Vector3<T>::rotate(enum Rotation rotation) @@ -159,16 +161,6 @@ void Vector3<T>::rotate(enum Rotation rotation)
tmp = z; z = x; x = -tmp;
return;
}
case ROTATION_PITCH_180_YAW_90: {
z = -z;
tmp = -x; x = -y; y = tmp;
return;
}
case ROTATION_PITCH_180_YAW_270: {
x = -x; z = -z;
tmp = x; x = y; y = -tmp;
return;
}
case ROTATION_ROLL_90_PITCH_90: {
tmp = z; z = y; y = -tmp;
tmp = z; z = -x; x = tmp;

Loading…
Cancel
Save