void
q_xyz_quat_invert(q_xyz_quat_type *destPtr, q_xyz_quat_type *srcPtr)
{
/* invert rotation first */
q_invert(destPtr->quat, srcPtr->quat);
/* vec = -vec */
q_vec_invert(destPtr->xyz, srcPtr->xyz);
/* rotate translation offsets into inverted system */
q_xform(destPtr->xyz, destPtr->quat, destPtr->xyz);
} /* q_xyz_quat_invert */
int
main(int argc, char *argv[])
{
q_type multQuat;
q_type xformedPoint;
q_type point;
double matrix[4][4];
/*
* read in, echo, and normalize 2 quaternions
*/
printf("\nEnter xform quaternion: (vec, s) ");
scanf("%lf %lf %lf %lf",
&multQuat[0], &multQuat[1], &multQuat[2], &multQuat[3]);
printf("\nEnter point: (x y z) ");
scanf("%lf %lf %lf",
&point[Q_X], &point[Q_Y], &point[Q_Z]);
point[Q_W] = 0.0;
/*
* matrix of product quat
*/
q_to_col_matrix(matrix, multQuat);
printf("Transformation (column) matrix:\n");
q_print_matrix(matrix);
/*
* xformedPoint = (multQuat * candQuat) * invertedQuat
*/
q_xform(xformedPoint, multQuat, point);
printf("Xform Result:\n");
q_print(xformedPoint);
return(0);
} /* main */
void
q_xyz_quat_compose(q_xyz_quat_type *C_from_A_ptr,
q_xyz_quat_type *C_from_B_ptr,
q_xyz_quat_type *B_from_A_ptr)
{
q_vec_type rotated_BA_vec;
/* rotate local xlate into global */
q_xform(rotated_BA_vec, C_from_B_ptr->quat, B_from_A_ptr->xyz);
/* now add the xformed local vec to the unchanged global vec */
q_vec_add(C_from_A_ptr->xyz, C_from_B_ptr->xyz, rotated_BA_vec);
/* compose the rotations */
/* CA_rotate = CB_rotate . BA_rotate */
q_mult(C_from_A_ptr->quat, C_from_B_ptr->quat, B_from_A_ptr->quat);
q_normalize(C_from_A_ptr->quat, C_from_A_ptr->quat);
} /* qp_xyz_quat_compose */
void vrpn_Tracker_WiimoteHead::_update_flip_state() {
if (d_flipState != FLIP_UNKNOWN) {
return;
}
q_vec_type upVec = {0, 1, 0};
q_xform(upVec, d_currentPose.quat, upVec);
if (upVec[1] < 0) {
// We are upside down - we will need to rotate 180 about the sensor Z
// Must recalculate now.
#ifdef VERBOSE
fprintf(stderr, "vrpn_Tracker_WiimoteHead: d_flipState = FLIP_180\n");
#endif
d_flipState = FLIP_180;
update_pose();
} else {
// OK, we are fine - there is a positive Y component to our up vector
#ifdef VERBOSE
fprintf(stderr, "vrpn_Tracker_WiimoteHead: d_flipState = FLIP_NORMAL\n");
#endif
d_flipState = FLIP_NORMAL;
}
}
void vrpn_IMU_SimpleCombiner::update_matrix_based_on_values(double time_interval)
{
//==================================================================
// Adjust the orientation based on the rotational velocity, which is
// measured in the rotated coordinate system. We need to rotate the
// difference vector back to the canonical orientation, apply the
// orientation change there, and then rotate back.
// Be sure to scale by the time value.
q_type forward, inverse;
q_copy(forward, d_quat);
q_invert(inverse, forward);
// Remember that Euler angles in Quatlib have rotation around Z in
// the first term. Compute the time-scaled delta transform in
// canonical space.
q_type delta;
q_from_euler(delta,
time_interval * d_rotational_vel.values[Q_Z],
time_interval * d_rotational_vel.values[Q_Y],
time_interval * d_rotational_vel.values[Q_X]);
// Bring the delta back into canonical space
q_type canonical;
q_mult(canonical, delta, inverse);
q_mult(canonical, forward, canonical);
q_mult(d_quat, canonical, d_quat);
//==================================================================
// To the extent that the acceleration vector's magnitude is equal
// to the expected gravity, rotate the orientation so that the vector
// points downward. This is measured in the rotated coordinate system,
// so we need to rotate back to canonical orientation and apply
// the difference there and then rotate back. The rate of rotation
// should be as specified in the gravity-rotation-rate parameter so
// we don't swing the head around too quickly but only slowly re-adjust.
double accel = sqrt(
d_acceleration.values[0] * d_acceleration.values[0] +
d_acceleration.values[1] * d_acceleration.values[1] +
d_acceleration.values[2] * d_acceleration.values[2] );
double diff = fabs(accel - 9.80665);
// Only adjust if we're close enough to the expected gravity
// @todo In a more complicated IMU tracker, base this on state and
// error estimates from a Kalman or other filter.
double scale = 1.0 - diff;
if (scale > 0) {
// Get a new forward and inverse matrix from the current, just-
// rotated matrix.
q_copy(forward, d_quat);
// Change how fast we adjust based on how close we are to the
// expected value of gravity. Then further scale this by the
// amount of time since the last estimate.
double gravity_scale = scale * d_gravity_restore_rate * time_interval;
// Rotate the gravity vector by the estimated transform.
// We expect this direction to match the global down (-Y) vector.
q_vec_type gravity_global;
q_vec_set(gravity_global, d_acceleration.values[0],
d_acceleration.values[1], d_acceleration.values[2]);
q_vec_normalize(gravity_global, gravity_global);
q_xform(gravity_global, forward, gravity_global);
//printf(" XXX Gravity: %lg, %lg, %lg\n", gravity_global[0], gravity_global[1], gravity_global[2]);
// Determine the rotation needed to take gravity and rotate
// it into the direction of -Y.
q_vec_type neg_y;
q_vec_set(neg_y, 0, -1, 0);
q_type rot;
q_from_two_vecs(rot, gravity_global, neg_y);
// Scale the rotation by the fraction of the orientation we
// should remove based on the time that has passed, how well our
// gravity vector matches expected, and the specified rate of
// correction.
static q_type identity = { 0, 0, 0, 1 };
q_type scaled_rot;
q_slerp(scaled_rot, identity, rot, gravity_scale);
//printf("XXX Scaling gravity vector by %lg\n", gravity_scale);
// Rotate by this angle.
q_mult(d_quat, scaled_rot, d_quat);
//==================================================================
// If we are getting compass data, and to the extent that the
// acceleration vector's magnitude is equal to the expected gravity,
// compute the cross product of the cross product to find the
// direction of north perpendicular to down. This is measured in
// the rotated coordinate system, so we need to rotate back to the
// canonical orientation and do the comparison there.
// The fraction of rotation should be as specified in the
// magnetometer-rotation-rate parameter so we don't swing the head
// around too quickly but only slowly re-adjust.
if (d_magnetometer.ana != NULL) {
// Get a new forward and inverse matrix from the current, just-
// rotated matrix.
q_copy(forward, d_quat);
// Find the North vector that is perpendicular to gravity by
// clearing its Y axis to zero and renormalizing.
q_vec_type magnetometer;
q_vec_set(magnetometer, d_magnetometer.values[0],
d_magnetometer.values[1], d_magnetometer.values[2]);
q_vec_type magnetometer_global;
q_xform(magnetometer_global, forward, magnetometer);
magnetometer_global[Q_Y] = 0;
q_vec_type north_global;
q_vec_normalize(north_global, magnetometer_global);
//printf(" XXX north_global: %lg, %lg, %lg\n", north_global[0], north_global[1], north_global[2]);
// Determine the rotation needed to take north and rotate it
// into the direction of negative Z.
q_vec_type neg_z;
q_vec_set(neg_z, 0, 0, -1);
q_from_two_vecs(rot, north_global, neg_z);
// Change how fast we adjust based on how close we are to the
// expected value of gravity. Then further scale this by the
// amount of time since the last estimate.
double north_rate = scale * d_north_restore_rate * time_interval;
// Scale the rotation by the fraction of the orientation we
// should remove based on the time that has passed, how well our
// gravity vector matches expected, and the specified rate of
// correction.
static q_type identity = { 0, 0, 0, 1 };
q_slerp(scaled_rot, identity, rot, north_rate);
// Rotate by this angle.
q_mult(d_quat, scaled_rot, d_quat);
}
}
//==================================================================
// Compute and store the velocity information
// This will be in the rotated coordinate system, so we need to
// rotate back to the identity orientation before storing.
// Convert from radians/second into a quaternion rotation as
// rotated in a hundredth of a second and set the rotational
// velocity dt to a hundredth of a second so that we don't
// risk wrapping.
// Remember that Euler angles in Quatlib have rotation around Z in
// the first term. Compute the time-scaled delta transform in
// canonical space.
q_from_euler(delta,
1e-2 * d_rotational_vel.values[Q_Z],
1e-2 * d_rotational_vel.values[Q_Y],
1e-2 * d_rotational_vel.values[Q_X]);
// Bring the delta back into canonical space
q_mult(canonical, delta, inverse);
q_mult(vel_quat, forward, canonical);
vel_quat_dt = 1e-2;
}
