diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp index 7a04dcbeaa..9760a1aac4 100644 --- a/libraries/AP_Math/quaternion.cpp +++ b/libraries/AP_Math/quaternion.cpp @@ -21,6 +21,11 @@ #include "AP_Math.h" #include +#define HALF_SQRT_2_PlUS_SQRT_2 0.92387953251128673848313610506011 // sqrt(2 + sqrt(2)) / 2 +#define HALF_SQRT_2_MINUS_SQTR_2 0.38268343236508972626808144923416 // sqrt(2 - sqrt(2)) / 2 +#define HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO 0.65328148243818828788676000840496 // sqrt((2 + sqrt(2))/2) / 2 +#define HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO 0.27059805007309845059637609665515 // sqrt((2 - sqrt(2))/2) / 2 + // return the rotation matrix equivalent for this quaternion template void QuaternionT::rotation_matrix(Matrix3d &m) const @@ -137,9 +142,9 @@ void QuaternionT::from_rotation(enum Rotation rotation) return; case ROTATION_YAW_45: - q1 = 0.92387956f; + q1 = HALF_SQRT_2_PlUS_SQRT_2; q2 = q3 = 0; - q4 = 0.38268343f; + q4 = HALF_SQRT_2_MINUS_SQTR_2; return; case ROTATION_YAW_90: @@ -149,9 +154,9 @@ void QuaternionT::from_rotation(enum Rotation rotation) return; case ROTATION_YAW_135: - q1 = 0.38268343f; + q1 = HALF_SQRT_2_MINUS_SQTR_2; q2 = q3 = 0; - q4 = 0.92387956f; + q4 = HALF_SQRT_2_PlUS_SQRT_2; return; case ROTATION_YAW_180: @@ -160,9 +165,9 @@ void QuaternionT::from_rotation(enum Rotation rotation) return; case ROTATION_YAW_225: - q1 = -0.38268343f; + q1 = -HALF_SQRT_2_MINUS_SQTR_2; q2 = q3 = 0; - q4 = 0.92387956f; + q4 = HALF_SQRT_2_PlUS_SQRT_2; return; case ROTATION_YAW_270: @@ -172,9 +177,9 @@ void QuaternionT::from_rotation(enum Rotation rotation) return; case ROTATION_YAW_315: - q1 = 0.92387956f; + q1 = HALF_SQRT_2_PlUS_SQRT_2; q2 = q3 = 0; - q4 = -0.38268343f; + q4 = -HALF_SQRT_2_MINUS_SQTR_2; return; case ROTATION_ROLL_180: @@ -184,8 +189,8 @@ void QuaternionT::from_rotation(enum Rotation rotation) case ROTATION_ROLL_180_YAW_45: q1 = q4 = 0; - q2 = 0.92387956f; - q3 = 0.38268343f; + q2 = HALF_SQRT_2_PlUS_SQRT_2; + q3 = HALF_SQRT_2_MINUS_SQTR_2; return; case ROTATION_ROLL_180_YAW_90: @@ -196,8 +201,8 @@ void QuaternionT::from_rotation(enum Rotation rotation) case ROTATION_ROLL_180_YAW_135: q1 = q4 = 0; - q2 = 0.38268343f; - q3 = 0.92387956f; + q2 = HALF_SQRT_2_MINUS_SQTR_2; + q3 = HALF_SQRT_2_PlUS_SQRT_2; return; case ROTATION_PITCH_180: @@ -207,8 +212,8 @@ void QuaternionT::from_rotation(enum Rotation rotation) case ROTATION_ROLL_180_YAW_225: q1 = q4 = 0; - q2 = -0.38268343f; - q3 = 0.92387956f; + q2 = -HALF_SQRT_2_MINUS_SQTR_2; + q3 = HALF_SQRT_2_PlUS_SQRT_2; return; case ROTATION_ROLL_180_YAW_270: @@ -220,8 +225,8 @@ void QuaternionT::from_rotation(enum Rotation rotation) case ROTATION_ROLL_180_YAW_315: q1 = q4 = 0; - q2 = 0.92387956f; - q3 = -0.38268343f; + q2 = HALF_SQRT_2_PlUS_SQRT_2; + q3 = -HALF_SQRT_2_MINUS_SQTR_2; return; case ROTATION_ROLL_90: @@ -230,9 +235,9 @@ void QuaternionT::from_rotation(enum Rotation rotation) return; case ROTATION_ROLL_90_YAW_45: - q1 = 0.65328151f; - q2 = 0.65328145f; - q3 = q4 = 0.27059802f; + q1 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO; + q2 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO; + q3 = q4 = HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO; return; case ROTATION_ROLL_90_YAW_90: @@ -240,9 +245,9 @@ void QuaternionT::from_rotation(enum Rotation rotation) return; case ROTATION_ROLL_90_YAW_135: - q1 = q2 = 0.27059802f; - q3 = 0.65328145f; - q4 = 0.65328151f; + q1 = q2 = HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO; + q3 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO; + q4 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO; return; case ROTATION_ROLL_270: @@ -252,10 +257,10 @@ void QuaternionT::from_rotation(enum Rotation rotation) return; case ROTATION_ROLL_270_YAW_45: - q1 = 0.65328151f; - q2 = -0.65328145f; - q3 = -0.27059802f; - q4 = 0.27059802f; + q1 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO; + q2 = -HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO; + q3 = -HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO; + q4 = HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO; return; case ROTATION_ROLL_270_YAW_90: @@ -264,10 +269,10 @@ void QuaternionT::from_rotation(enum Rotation rotation) return; case ROTATION_ROLL_270_YAW_135: - q1 = 0.27059802f; - q2 = -0.27059802f; - q3 = -0.65328145f; - q4 = 0.65328151f; + q1 = HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO; + q2 = -HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO; + q3 = -HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO; + q4 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO; return; case ROTATION_PITCH_90: @@ -334,40 +339,40 @@ void QuaternionT::from_rotation(enum Rotation rotation) return; case ROTATION_ROLL_90_PITCH_68_YAW_293: - q1 = 0.26774535f; - q2 = 0.70698798f; - q3 = 0.01295743f; - q4 = -0.65445596f; + q1 = 0.26774500501681575137524760066299; + q2 = 0.70698804688952421315661922562867; + q3 = 0.012957683254962659713527273197542; + q4 = -0.65445596665363614530264158020145; return; case ROTATION_PITCH_315: - q1 = 0.92387956f; + q1 = HALF_SQRT_2_PlUS_SQRT_2; q2 = q4 = 0; - q3 = -0.38268343f; + q3 = -HALF_SQRT_2_MINUS_SQTR_2; return; case ROTATION_ROLL_90_PITCH_315: - q1 = 0.65328151f; - q2 = 0.65328145f; - q3 = -0.27059802f; - q4 = 0.27059802f; + q1 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO; + q2 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO; + q3 = -HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO; + q4 = HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO; return; case ROTATION_PITCH_7: - q1 = 0.99813479f; + q1 = 0.99813479842186692003735970502021; q2 = q4 = 0; - q3 = 0.06104854f; + q3 = 0.061048539534856872956769535676358; return; case ROTATION_ROLL_45: - q1 = 0.9238795325112867; - q2 = 0.3826834323650898; + q1 = HALF_SQRT_2_PlUS_SQRT_2; + q2 = HALF_SQRT_2_MINUS_SQTR_2; q3 = q4 = 0.0; return; case ROTATION_ROLL_315: - q1 = 0.9238795325112867; - q2 = -0.3826834323650898; + q1 = HALF_SQRT_2_PlUS_SQRT_2; + q2 = -HALF_SQRT_2_MINUS_SQTR_2; q3 = q4 = 0.0; return; diff --git a/libraries/AP_Math/rotations.h b/libraries/AP_Math/rotations.h index 864d371fa6..d7b4dabbba 100644 --- a/libraries/AP_Math/rotations.h +++ b/libraries/AP_Math/rotations.h @@ -63,7 +63,7 @@ enum Rotation : uint8_t { ROTATION_ROLL_270_PITCH_270 = 35, ROTATION_ROLL_90_PITCH_180_YAW_90 = 36, ROTATION_ROLL_90_YAW_270 = 37, - ROTATION_ROLL_90_PITCH_68_YAW_293 = 38, + ROTATION_ROLL_90_PITCH_68_YAW_293 = 38, // this is actually, roll 90, pitch 68.8, yaw 293.3 ROTATION_PITCH_315 = 39, ROTATION_ROLL_90_PITCH_315 = 40, ROTATION_PITCH_7 = 41, @@ -81,7 +81,7 @@ enum Rotation : uint8_t { // definitions used by quaterion and vector3f -#define HALF_SQRT_2 0.70710678118654757f +#define HALF_SQRT_2 0.70710678118654752440084436210485 /* Here are the same values in a form suitable for a @Values attribute in diff --git a/libraries/AP_Math/tests/test_math.cpp b/libraries/AP_Math/tests/test_math.cpp index 584d33ce3b..15981f1248 100644 --- a/libraries/AP_Math/tests/test_math.cpp +++ b/libraries/AP_Math/tests/test_math.cpp @@ -10,7 +10,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); -#define SQRT_2 1.4142135623730951f +#define SQRT_2 1.4142135623730950488016887242097 TEST(VectorTest, Rotations) { @@ -74,10 +74,10 @@ TEST(VectorTest, Rotations) TEST_ROTATION(ROTATION_ROLL_270_PITCH_270, 1, 1, 1); TEST_ROTATION(ROTATION_ROLL_90_PITCH_180_YAW_90, 1, -1, -1); TEST_ROTATION(ROTATION_ROLL_90_YAW_270, -1, -1, 1); - TEST_ROTATION(ROTATION_ROLL_90_PITCH_68_YAW_293, -0.4066309f, -1.5839677f, -0.5706992f); + TEST_ROTATION(ROTATION_ROLL_90_PITCH_68_YAW_293, -0.40663092252764576617352076937095, -1.5839677018260314156350432313047, -0.57069923113341980425161636958364); TEST_ROTATION(ROTATION_PITCH_315, 0, 1, SQRT_2); TEST_ROTATION(ROTATION_ROLL_90_PITCH_315, 0, -1, SQRT_2); - TEST_ROTATION(ROTATION_PITCH_7, 1.1144155f, 1, 0.87067682f); + TEST_ROTATION(ROTATION_PITCH_7, 1.1144154950464695286171945554088, 1, 0.87067680823617454866081288855639); TEST_ROTATION(ROTATION_ROLL_45, 1, 0, SQRT_2); TEST_ROTATION(ROTATION_ROLL_315, 1, SQRT_2, 0); EXPECT_EQ(ROTATION_MAX, rotation_count) << "All rotations are expect to be tested"; diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index cee78b9801..00a2248b7b 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -216,9 +216,9 @@ void Vector3::rotate(enum Rotation rotation) T tmpx = x; T tmpy = y; T tmpz = z; - x = 0.143039f * tmpx + 0.368776f * tmpy + -0.918446f * tmpz; - y = -0.332133f * tmpx + -0.856289f * tmpy + -0.395546f * tmpz; - z = -0.932324f * tmpx + 0.361625f * tmpy + 0.000000f * tmpz; + x = 0.14303897231223747232853327204793 * tmpx + 0.36877648650320382639478111741482 * tmpy + -0.91844638134308709265241077446262 * tmpz; + y = -0.33213277779664740485543461545603 * tmpx + -0.85628942146641884303193137384369 * tmpy + -0.39554550256296522325882847326284 * tmpz; + z = -0.93232380121551217122544130688766 * tmpx + 0.36162457008209242248497616856184 * tmpy + 0.00000000000000002214311861220361 * tmpz; return; } case ROTATION_PITCH_315: { @@ -235,8 +235,8 @@ void Vector3::rotate(enum Rotation rotation) return; } case ROTATION_PITCH_7: { - const T sin_pitch = 0.12186934340514748f; // sinF(pitch); - const T cos_pitch = 0.992546151641322f; // cosF(pitch); + const T sin_pitch = 0.1218693434051474899781908334262; // sinF(pitch); + const T cos_pitch = 0.99254615164132198312785249072476; // cosF(pitch); T tmpx = x; T tmpz = z; x = cos_pitch * tmpx + sin_pitch * tmpz;