static void on_mouse_move(struct glwin *win, int x, int y)
{
struct quaternion next;
quaternion_mul(&q_delta, &q_cur, &next);
if (g_mouse_down[0]) {
float axis[3];
axis[0] = g_dy[0] - y;
axis[1] = g_dx[0] - x;
axis[2] = 0;
float norm = sqrtf((axis[0] * axis[0]) + (axis[1] * axis[1]) + (axis[2] * axis[2]));
if (norm > 0) {
axis[0] /= norm;
axis[1] /= norm;
axis[2] /= norm;
}
quaternion_from_axis_angle(&q_delta, axis[0], axis[1], axis[2], norm * 3 / win->width);
g_need_redraw = true;
}
if (g_mouse_down[2]) {
float delta[3];
delta[0] = (g_dx[2] - x) * 2.0 / win->width;
delta[1] = -(g_dy[2] - y) * 2.0 / win->height;
delta[2] = 0;
quaternion_unit_inv_mul_vec3(&next, delta, g_offset_next);
g_offset_next[0] = -g_offset_next[0];
g_offset_next[1] = -g_offset_next[1];
g_offset_next[2] = -g_offset_next[2];
g_need_redraw = true;
}
}
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;
}
