static bool math_eigen_test_rotation_method_consistency(const Eigen::Quaternionf &q, const Eigen::Vector3f &v)
{
bool success = true;
assert_eigen_quaternion_is_normalized(q);
// This is the same as computing the rotation by computing: q^-1*[0,v]*q, but cheaper
Eigen::Vector3f v_rotated = eigen_vector3f_clockwise_rotate(q, v);
// Make sure doing the matrix based rotation performs the same result
{
Eigen::Matrix3f m = eigen_quaternion_to_clockwise_matrix3f(q);
Eigen::Vector3f v_test = m * v;
success &= v_test.isApprox(v_rotated, k_normal_epsilon);
assert(success);
}
// Make sure the Hamilton product style rotation matches
{
Eigen::Quaternionf v_as_quaternion = Eigen::Quaternionf(0.f, v.x(), v.y(), v.z());
Eigen::Quaternionf q_inv = q.conjugate();
Eigen::Quaternionf qinv_v = q_inv * v_as_quaternion;
Eigen::Quaternionf qinv_v_q = qinv_v * q;
success &=
is_nearly_equal(qinv_v_q.w(), 0.f, k_normal_epsilon) &&
is_nearly_equal(qinv_v_q.x(), v_rotated.x(), k_normal_epsilon) &&
is_nearly_equal(qinv_v_q.y(), v_rotated.y(), k_normal_epsilon) &&
is_nearly_equal(qinv_v_q.z(), v_rotated.z(), k_normal_epsilon);
assert(success);
}
return success;
}
void QuaternionDemo::paintGL()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(60.0, (double)width() / height(), 0.1, 10);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glTranslatef(0, 0, -5);
glRotatef(-90, 1, 0, 0);
glRotatef(90, 0, 0, 1);
Eigen::Transform<float, 3, Eigen::Affine> t_ref(ref.conjugate());
glMultMatrixf(t_ref.data());
Eigen::Transform<float, 3, Eigen::Affine> t_q(q);
glMultMatrixf(t_q.data());
glEnable(GL_DEPTH_TEST);
glBegin(GL_QUADS);
glColor3f(1, 1, 1);
glVertex3f(-1, -1, -1);
glVertex3f(1, -1, -1);
glVertex3f(1, 1, -1);
glVertex3f(-1, 1, -1);
glColor3f(1, 0, 0);
glVertex3f(-1, -1, 1);
glVertex3f(1, -1, 1);
glVertex3f(1, 1, 1);
glVertex3f(-1, 1, 1);
glColor3f(0, 1, 0);
glVertex3f(-1, -1, 1);
glVertex3f(-1, -1, -1);
glVertex3f(-1, 1, -1);
glVertex3f(-1, 1, 1);
glColor3f(1, 0.5f, 0);
glVertex3f(1, -1, 1);
glVertex3f(1, -1, -1);
glVertex3f(1, 1, -1);
glVertex3f(1, 1, 1);
glColor3f(0, 0, 1);
glVertex3f(-1, -1, 1);
glVertex3f(-1, -1, -1);
glVertex3f(1, -1, -1);
glVertex3f(1, -1, 1);
glColor3f(1, 0, 0.5f);
glVertex3f(-1, 1, 1);
glVertex3f(-1, 1, -1);
glVertex3f(1, 1, -1);
glVertex3f(1, 1, 1);
glEnd();
}
static Eigen::Vector3f
quaternion_derivative_to_angular_velocity(
const Eigen::Quaternionf ¤t_orientation,
const Eigen::Quaternionf &quaternion_derivative)
{
Eigen::Quaternionf inv_orientation = current_orientation.conjugate();
auto q_ang_vel = (quaternion_derivative*inv_orientation).coeffs() * 2.f;
Eigen::Vector3f ang_vel(q_ang_vel.x(), q_ang_vel.y(), q_ang_vel.z());
return ang_vel;
}