/**
* @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));
}
