void vrpn_Sound_Server_Miles::GetCurrentOrientation(const vrpn_int32 CurrentSoundId,F32 *X_val, F32 *Y_val, F32 *Z_val) {
F32 X_f, Y_f, Z_f, X_u, Y_u, Z_u;
q_vec_type angles;
q_matrix_type colMatrix;
q_type srcQuat;
if ((CurrentSoundId > -1) && (provider != 0)) {
AIL_3D_orientation(getSample(CurrentSoundId),&X_f,&Y_f,&Z_f,&X_u,&Y_u,&Z_u);
srcQuat[0] = X_f;
srcQuat[1] = Y_f;
srcQuat[2] = Z_f;
srcQuat[3] = 1.0;
fprintf(stderr,"%f %f %f\n",srcQuat[0]*180.0/Q_PI,srcQuat[1]*180.0/Q_PI,srcQuat[2]*180.0/Q_PI);
q_to_col_matrix (colMatrix, srcQuat);
q_col_matrix_to_euler(angles,colMatrix);
*X_val = angles[0]*180.0/Q_PI;
*Y_val = angles[1]*180.0/Q_PI;
*Z_val = angles[2]*180.0/Q_PI;
}
return;
}
void vrpn_Sound_Server_Miles::GetListenerOrientation(F32 *X_val, F32 *Y_val, F32 *Z_val) {
q_vec_type angles;
q_matrix_type colMatrix;
q_type srcQuat;
F32 X_f, Y_f, Z_f, X_u, Y_u, Z_u;
if (provider != 0) {
AIL_3D_orientation(listener,&X_f,&Y_f,&Z_f,&X_u,&Y_u,&Z_u);
srcQuat[0] = X_f;
srcQuat[1] = Y_f;
srcQuat[2] = Z_f;
srcQuat[3] = 1.0;
q_to_col_matrix (colMatrix, srcQuat);
q_col_matrix_to_euler(angles,colMatrix);
*X_val = angles[0]*180.0/Q_PI;
*Y_val = angles[1]*180.0/Q_PI;
*Z_val = angles[2]*180.0/Q_PI;
}
return;
}
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 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();
}
