/** * @ingroup experimental * @brief Computes the angle between two quaternions * * * \f$angle= 2 * acos((self \cdot qT) / (||self|| * ||qT||))\f$ * */ HYPAPI float quaternion_angle_between_EXP(const quaternion *self, const quaternion *qT) { float c; /* cosine */ c = quaternion_dot_product(self, qT) / ( quaternion_norm(self) * quaternion_norm(qT) ); return 2.0f * HYP_ACOS(c); }
TEST_F(QuaternionTest, inverse_of_a_quaternion_is_the_conjugate_divded_by_the_norm_squared) { const auto quat = create_random_quaternion(); const auto res = quaternion_inverse(quat); const auto normsquared = quaternion_norm(quat) * quaternion_norm(quat); const auto inverse = quaternion_conjugate(quat) / normsquared; EXPECT_FLOAT_EQ(inverse.w(), res.w()); EXPECT_FLOAT_EQ(inverse.x(), res.x()); EXPECT_FLOAT_EQ(inverse.y(), res.y()); EXPECT_FLOAT_EQ(inverse.z(), res.z()); }
TEST_F(QuaternionTest, normalizing_quaternion_gives_a_unit_quaternion) { auto quat = create_random_quaternion(); quaternion_normalize(quat); EXPECT_FLOAT_EQ(1, quaternion_norm(quat)); }
/** * @ingroup quaternion * @brief Calculates the inverse of the quaternion * * \f$q^{-1} = (q')/\|q\|\f$ * * inverse = conjugate(q) / norm(q) * * The inverse can be checked by multiplying the inverse against the original quaternion. The result should be the identity. * * @snippet test_quaternion.c quaternion inverse example */ HYPAPI quaternion * quaternion_inverse(quaternion *self) { float norm; norm = quaternion_norm(self); if(norm == 0.0f) { /* avoid divide by zero */ return self; } quaternion_conjugate(self); if(norm == 1.0f) { /* we're done */ return self; } self->x /= norm; self->y /= norm; self->z /= norm; self->w /= norm; return self; }
/** * @ingroup quaternion * @brief Given two vectors, find a quaternion that will get you from one to the other * * @param from the starting vector * @param to the ending vector * @param qR the resulting quaternion that gets you from the starting vector to the ending vector */ HYPAPI quaternion * quaternion_get_rotation_tov3(const vector3 *from, const vector3 *to, quaternion *qR) { /* this code avoids sqrt and cos and sin and would be nice to avoid division */ vector3 w; float dot; float norm; vector3_cross_product(&w, from, to); dot = vector3_dot_product(from, to); qR->x = w.x; qR->y = w.y; qR->z = w.z; qR->w = dot; norm = quaternion_norm(qR); qR->w += norm; /* normalization with avoidance of div/0 and reusing the norm (already calculated above) */ if(norm != 0) { qR->x /= norm; qR->y /= norm; qR->z /= norm; qR->w /= norm; } return qR; }
/** * @ingroup experimental * @brief returns a score that seeks to describe the difference between two quaternions * */ HYPAPI float quaternion_difference_EXP(const quaternion *q1, const quaternion *q2) { quaternion diff; diff.x = q2->x - q1->x; diff.y = q2->y - q1->y; diff.z = q2->z - q1->z; diff.w = q2->w - q1->w; return quaternion_norm(&diff); }
TEST_F(QuaternionTest, norm_of_quaternion_with_only_real_element_set_is_the_real_element) { auto quat = create_random_quaternion(); quat.x() = 0; quat.y() = 0; quat.z() = 0; EXPECT_EQ(quat.w(), quaternion_norm(quat)); }
TEST_F(QuaternionTest, norm_of_quaternion_is_the_square_root_of_the_sum_of_the_squared_elements) { const auto quat = create_random_quaternion(); auto res = std::sqrt(quat.w() * quat.w() + quat.x() * quat.x() + quat.y() * quat.y() + quat.z() * quat.z()); EXPECT_FLOAT_EQ(res, quaternion_norm(quat)); }
void quaternion_normalize(Quaternion *quat) { double n = quaternion_norm(quat); if (APPROX(n, 1)) { return; } quat->w /= n; quat->x /= n; quat->y /= n; quat->z /= n; }
/** [quaternion identity example] */ static char * test_quaternion_identity() { quaternion q; quaternion_identity(&q); test_assert(q.x == 0.0f); test_assert(q.y == 0.0f); test_assert(q.z == 0.0f); test_assert(q.w == 1.0f); test_assert(1.0f == quaternion_norm(&q)); /* test_assert(0.0f == quaternion_magnitude(&q)); */ test_assert(quaternion_is_unit(&q)); test_assert(!quaternion_is_pure(&q)); return 0; }
Math::Matrix4d make_matrix_from_quaternion(const Math::Quaternion<double>& quat) { Math::Matrix4d matrix; const auto s = 2.0 / quaternion_norm(quat); matrix(0,0) -= s *(quat.y() * quat.y() + quat.z() * quat.z()); matrix(0,1) += s *(quat.x() * quat.y() - quat.w() * quat.z()); matrix(0,2) += s *(quat.x() * quat.z() + quat.w() * quat.y()); matrix(1,0) += s *(quat.x() * quat.y() + quat.w() * quat.z()); matrix(1,1) -= s *(quat.x() * quat.x() + quat.z() * quat.z()); matrix(1,2) += s *(quat.y() * quat.z() - quat.w() * quat.x()); matrix(2,0) += s *(quat.x() * quat.z() - quat.w() * quat.y()); matrix(2,1) += s *(quat.y() * quat.z() + quat.w() * quat.x()); matrix(2,2) -= s *(quat.x() * quat.x() + quat.y() * quat.y()); return matrix; }
quaternion ahrs_orientation_from_accel_mag(dataexchange_t *data) { //vectors for rotation matrix vector3d acc_v, mag_v; matrix3x3d rmat; quaternion q; acc_v.x = (*data).acc_x; acc_v.y = (*data).acc_y; acc_v.z = (*data).acc_z; mag_v.x = (*data).mag_x; mag_v.y = (*data).mag_y; mag_v.z = (*data).mag_z; hmc5883l_applyCalibration(&mag_v, calib_ptr); vector3d down = vector_inv(acc_v); vector3d east = vector_cross(down, mag_v); vector3d north = vector_cross(east, down); //normalize vectors vector_norm(&down); vector_norm(&east); vector_norm(&north); //vectors to matrix matrix_vector_to_row(&rmat, north, 1); matrix_vector_to_row(&rmat, east, 2); matrix_vector_to_row(&rmat, down, 3); //matrix to quaternion q = quaternion_from_matrix(&rmat); //normalize quaternion_norm(&q); (*data).qr = q; return q; }
quaternion ahrs_orientation_from_gyro(dataexchange_t *data) { //angle component vector3d gyr_rad_s = l3g4200d_raw_to_rad(data); double angle = vector_magnitude(gyr_rad_s) * (*data).time_period; //axis, normalized vector_norm(&gyr_rad_s); //quaternion from axis/angle quaternion q = quaternion_from_axis_angle(gyr_rad_s, angle); //normalize quaternion_norm(&q); (*data).qr = quaternion_product((*data).qr, q); return q; }
/** * @ingroup quaternion * @brief if the norm is 1.0, then the quaternion is said to be a *unit quaternion* */ HYPAPI short quaternion_is_unit(quaternion *self) { return (scalar_equalsf(1.0f, quaternion_norm(self))); }
void ahrs_init(dataexchange_t *data, u8g_t *disp, params_t *calib_params) { //vectors for absolute rotation matrix vector3d acc_v, mag_v; matrix3x3d initial_rmat; quaternion initial_q; calib_ptr = calib_params; int16_t amount = 100; //show init start message _delay_ms(2000); display_draw_line2x("Initialization,", "please stand still"); _delay_ms(2000); acc_v.x = 0.0; acc_v.y = 0.0; acc_v.z = 0.0; mag_v.x = 0.0; mag_v.y = 0.0; mag_v.z = 0.0; for (int i = 0; i < amount; i++) { adxl345_read(data); //accelerometer read hmc5883l_read(data); //magnetometer read acc_v.x += (*data).acc_x; acc_v.y += (*data).acc_y; acc_v.z += (*data).acc_z; mag_v.x += (*data).mag_x; mag_v.y += (*data).mag_y; mag_v.z += (*data).mag_z; // if ((i % 10) == 0) { // char str[3]; // sprintf(str, "ready: %d%%", i); // display_draw_line2x("Initialization:", str); // } } display_draw_line2x("Initialization:", "...done!"); _delay_ms(2000); display_draw_logo(); acc_v.x /= amount; acc_v.y /= amount; acc_v.z /= amount; mag_v.x /= amount; mag_v.y /= amount; mag_v.z /= amount; hmc5883l_applyCalibration(&mag_v, calib_ptr); vector3d down = vector_inv(acc_v); vector3d east = vector_cross(down, mag_v); vector3d north = vector_cross(east, down); //normalize vectors vector_norm(&down); vector_norm(&east); vector_norm(&north); //vectors to matrix matrix_vector_to_row(&initial_rmat, north, 1); matrix_vector_to_row(&initial_rmat, east, 2); matrix_vector_to_row(&initial_rmat, down, 3); //matrix to quaternion initial_q = quaternion_from_matrix(&initial_rmat); //normalize quaternion_norm(&initial_q); //initialize (*data).qr.w = initial_q.w; (*data).qr.x = initial_q.x; (*data).qr.y = initial_q.y; (*data).qr.z = initial_q.z; }
/** * @ingroup quaternion * @brief Calculates the magnitude of the quaternion * * The magnitude is the sqrt of the norm * * \f$\sqrt{\|q\|}\f$ */ HYPAPI float quaternion_magnitude(const quaternion *self) { return HYP_SQRT(quaternion_norm(self)); }
Quaternion quaternion_normalize(Quaternion q) { float factor = 1.0 / quaternion_norm(q); return quaternion_scale(q, factor); }
TEST_F(QuaternionTest, norm_of_quaternion_with_only_imaginary_part_is_the_length_of_the_imaginary_vector) { auto quat = create_random_quaternion(); quat.w() = 0; EXPECT_EQ(std::sqrt(quat.x() * quat.x() + quat.y() * quat.y() + quat.z() * quat.z()), quaternion_norm(quat)); }
TEST_F(QuaternionTest, norm_of_identity_quaternion_is_1) { const Math::Quaternion<double> quat; EXPECT_EQ(1, quaternion_norm(quat)); }