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(); }