// static
void vrpn_Tracker_JoyFly::handle_joystick
(void * userdata, const vrpn_ANALOGCB b)
{
double tx, ty, tz, rx, ry, rz;
double temp[7];
int i,j;
q_matrix_type newM;
double deltaT;
vrpn_Tracker_JoyFly * pts = (vrpn_Tracker_JoyFly *) userdata;
printf("Joy total = %d,Chan[0] = %lf\n",
b.num_channel,b.channel[0]);
if (pts->prevtime.tv_sec == -1) {
deltaT = 1.0 ; // one milisecond;
} else {
deltaT = (pts->prevtime.tv_sec* ONE_SEC + pts->prevtime.tv_usec) -
(b.msg_time.tv_sec *ONE_SEC + b.msg_time.tv_usec);
deltaT /= 1000.0 ; // convert to millsecond;
}
memcpy(&(pts->prevtime), &b.msg_time, sizeof(struct timeval));
for (i=0; i< 7; i++) {
temp[i] = 1.0;
//printf("chan[%d] = %lf\n", i, b.channel[i]);
if (!(b.channel[i] >=-0.5 && b.channel[i] <= 0.5)) return;
for (j=0; j< pts->chanPower[i]; j++)
temp[i] *= b.channel[i];
if (b.channel[i] > 0) {
temp[i] *= pts->chanAccel[i];
} else {
temp[i] = -fabs(temp[i]) * pts->chanAccel[i];
}
temp[i] *= deltaT;
}
/* roll chan[3] */
rz = temp[3];
/* pitch Chan[4]*/
rx = temp[4];
/* yaw Chan[5]*/
ry = temp[5];
/* translation, x chan[0], y: chane[2], z: chan[1] */
tx = -temp[0]; // tx is NEGTIVE of power !!! ;
ty = temp[2];
tz = temp[1];
q_euler_to_col_matrix(newM, rz, ry, rx);
newM[3][0] = tx; newM[3][1] = ty; newM[3][2] = tz;
pts->update(newM);
}
void vrpn_Tracker_ButtonFly::handle_button_update
(void *userdata, const vrpn_BUTTONCB info)
{
vrpn_TBF_fullaxis *axis = (vrpn_TBF_fullaxis*)userdata;
// If this is not the correct button for this axis, return
if (axis->axis.channel != info.button) {
return;
}
// If this is an absolute axis, and the button has just been pressed,
// then set the matrix to the one for this button.
if (axis->axis.absolute) {
if (info.state == 1) {
double tx,ty,tz, rx,ry,rz; //< Translation and rotation to set to
q_matrix_type newMatrix; //< Matrix set to these values.
// compute the translation and rotation
tx = axis->axis.vec[0];
ty = axis->axis.vec[1];
tz = axis->axis.vec[2];
rx = axis->axis.rot[0] * (2*M_PI);
ry = axis->axis.rot[1] * (2*M_PI);
rz = axis->axis.rot[2] * (2*M_PI);
// Build a rotation matrix, then add in the translation
q_euler_to_col_matrix(newMatrix, rz, ry, rx);
newMatrix[3][0] = tx; newMatrix[3][1] = ty; newMatrix[3][2] = tz;
// Copy the new matrix to the current matrix
// and then update the tracker based on it
q_matrix_copy(axis->bf->d_currentMatrix, newMatrix);
axis->bf->convert_matrix_to_tracker();
// Mark the axis as active, so that a report will be generated
// next time. For absolute channels, this is marked inactive when
// the report based on it is generated.
axis->active = true;
}
// This is a differential axis, so mark it as active or not
// depending on whether the button was pressed or not.
} else {
axis->active = (info.state != 0);
// XXX To be strictly correct, we should record the time at which
// the activity started so that it can take effect between update
// intervals.
}
}
void vrpn_Tracker_ButtonFly::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
int i;
// Set the translation and rotation to zero, then go through all of the
// axis and sum in the contributions from any that are differential and
// active.
tx = ty = tz = rx = ry = rz = 0.0;
for (i = 0; i < d_num_axes; i++) {
if (d_axes[i].active && !d_axes[i].axis.absolute) {
tx += d_axes[i].axis.vec[0] * time_interval * d_vel_scale_value;
ty += d_axes[i].axis.vec[1] * time_interval * d_vel_scale_value;
tz += d_axes[i].axis.vec[2] * time_interval * d_vel_scale_value;
rx = d_axes[i].axis.rot[0] * time_interval * (2*M_PI) * d_rot_scale_value;
ry = d_axes[i].axis.rot[1] * time_interval * (2*M_PI) * d_rot_scale_value;
rz = d_axes[i].axis.rot[2] * time_interval * (2*M_PI) * d_rot_scale_value;
}
}
// 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;
// Multiply the current matrix by the difference matrix to update
// it to the current time. Then convert the matrix into a pos/quat
// and copy it into the tracker position and quaternion structures.
q_matrix_type final;
q_matrix_mult(final, diffM, d_currentMatrix);
q_matrix_copy(d_currentMatrix, final);
convert_matrix_to_tracker();
}
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();
}
