void vrpn_Sound_Server_Miles::changeListenerStatus(vrpn_ListenerDef listenerDef)
//change pose, etc for listener
{
if (provider != 0) {
vrpn_float32 uX = 0, uY = 1, uZ = 0;
// switch to left handed
listenerDef.pose.orientation[2] *= -1.0;
listenerDef.pose.orientation[3] *= -1.0;
listenerDef.pose.position[2] *= -1.0;
AIL_set_3D_position(listener, listenerDef.pose.position[0], listenerDef.pose.position[1], listenerDef.pose.position[2]);
// normalize
listenerDef.pose.orientation[0] /= listenerDef.pose.orientation[3];
listenerDef.pose.orientation[1] /= listenerDef.pose.orientation[3];
listenerDef.pose.orientation[2] /= listenerDef.pose.orientation[3];
listenerDef.pose.orientation[3] /= listenerDef.pose.orientation[3];
q_matrix_type mymatrix;
q_matrix_type multmatrix;
q_type facevec;
q_to_row_matrix(mymatrix, listenerDef.pose.orientation);
// 90 degree rotation about x
multmatrix[0][0] = 1;
multmatrix[0][1] = 0;
multmatrix[0][2] = 0;
multmatrix[0][3] = 0;
multmatrix[1][0] = 0;
multmatrix[1][1] = 0;
multmatrix[1][2] = -1;
multmatrix[1][3] = 0;
multmatrix[2][0] = 0;
multmatrix[2][1] = 1;
multmatrix[2][2] = 0;
multmatrix[2][3] = 0;
multmatrix[3][0] = 0;
multmatrix[3][1] = 0;
multmatrix[3][2] = 0;
multmatrix[3][3] = 1;
q_matrix_mult(mymatrix, mymatrix, multmatrix);
q_from_row_matrix(facevec, mymatrix);
AIL_set_3D_orientation(listener, listenerDef.pose.orientation[0], listenerDef.pose.orientation[1], listenerDef.pose.orientation[2], facevec[0], facevec[1], facevec[2]);
AIL_set_3D_velocity(listener, listenerDef.velocity[0], listenerDef.velocity[1], listenerDef.velocity[2], listenerDef.velocity[3]);
} else fprintf(stderr,"No provider has been set prior to changeListenerStatus\n");
}
void vrpn_Tracker_AnalogFly::update_matrix_based_on_values
(double time_interval)
{
double tx,ty,tz, rx,ry,rz; // Translation (m/s) and rotation (rad/sec)
q_matrix_type diffM; // Difference (delta) matrix
// For absolute trackers, the interval is treated as "1", so that the
// translations and rotations are unscaled;
if (d_absolute) { time_interval = 1.0; };
// compute the translation and rotation
tx = d_x.value * time_interval;
ty = d_y.value * time_interval;
tz = d_z.value * time_interval;
rx = d_sx.value * time_interval * (2*VRPN_PI);
ry = d_sy.value * time_interval * (2*VRPN_PI);
rz = d_sz.value * time_interval * (2*VRPN_PI);
// Build a rotation matrix, then add in the translation
q_euler_to_col_matrix(diffM, rz, ry, rx);
diffM[3][0] = tx; diffM[3][1] = ty; diffM[3][2] = tz;
// While the clutch is not engaged, we don't move. Record that
// the clutch was off so that we know later when it is re-engaged.
if (!d_clutch_engaged) {
d_clutch_was_off = true;
return;
}
// When the clutch becomes re-engaged, we store the current matrix
// multiplied by the inverse of the present differential matrix so that
// the first frame of the mouse-hold leaves us in the same location.
// For the absolute matrix, this re-engages new motion at the previous
// location.
if (d_clutch_engaged && d_clutch_was_off) {
d_clutch_was_off = false;
q_type diff_orient;
// This is backwards, because Euler angles have rotation about Z first...
q_from_euler(diff_orient, rz, ry, rx);
q_xyz_quat_type diff;
q_vec_set(diff.xyz, tx, ty, tz);
q_copy(diff.quat, diff_orient);
q_xyz_quat_type diff_inverse;
q_xyz_quat_invert(&diff_inverse, &diff);
q_matrix_type di_matrix;
q_to_col_matrix(di_matrix, diff_inverse.quat);
di_matrix[3][0] = diff_inverse.xyz[0];
di_matrix[3][1] = diff_inverse.xyz[1];
di_matrix[3][2] = diff_inverse.xyz[2];
q_matrix_mult(d_clutchMatrix, di_matrix, d_currentMatrix);
}
// Apply the matrix.
if (d_absolute) {
// The difference matrix IS the current matrix. Catenate it
// onto the clutch matrix. If there is no clutching happening,
// this matrix will always be the identity so this will just
// copy the difference matrix.
q_matrix_mult(d_currentMatrix, diffM, d_clutchMatrix);
} else {
// Multiply the current matrix by the difference matrix to update
// it to the current time.
if (d_worldFrame) {
// If using world frame:
// 1. Separate out the translation and add to the differential translation
tx += d_currentMatrix[3][0];
ty += d_currentMatrix[3][1];
tz += d_currentMatrix[3][2];
diffM[3][0] = 0; diffM[3][1] = 0; diffM[3][2] = 0;
d_currentMatrix[3][0] = 0; d_currentMatrix[3][1] = 0; d_currentMatrix[3][2] = 0;
// 2. Compose the rotations.
q_matrix_mult(d_currentMatrix, d_currentMatrix, diffM);
// 3. Put the new translation back in the matrix.
d_currentMatrix[3][0] = tx; d_currentMatrix[3][1] = ty; d_currentMatrix[3][2] = tz;
} else {
q_matrix_mult(d_currentMatrix, diffM, d_currentMatrix);
}
}
// Finally, convert the matrix into a pos/quat
// and copy it into the tracker position and quaternion structures.
convert_matrix_to_tracker();
}
