void vrpn_Tracker_ButtonFly::reset (void)
{
// Set the matrix back to the identity matrix
q_matrix_copy(d_currentMatrix, d_initMatrix);
vrpn_gettimeofday(&d_prevtime, NULL);
// Convert the matrix into quaternion notation and copy into the
// tracker pos and quat elements.
convert_matrix_to_tracker();
}
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();
}
vrpn_Tracker_AnalogFly::vrpn_Tracker_AnalogFly
(const char * name, vrpn_Connection * trackercon,
vrpn_Tracker_AnalogFlyParam * params, float update_rate,
bool absolute, bool reportChanges,
bool worldFrame) :
vrpn_Tracker (name, trackercon),
d_update_interval (update_rate ? (1/update_rate) : 1.0),
d_absolute (absolute),
d_reportChanges (reportChanges),
d_worldFrame (worldFrame),
d_reset_button(NULL),
d_which_button (params->reset_which),
d_clutch_button(NULL),
d_clutch_which (params->clutch_which),
d_clutch_engaged(false),
d_clutch_was_off(false)
{
int i;
d_x.axis = params->x; d_y.axis = params->y; d_z.axis = params->z;
d_sx.axis = params->sx; d_sy.axis = params->sy; d_sz.axis = params->sz;
d_x.ana = d_y.ana = d_z.ana = NULL;
d_sx.ana = d_sy.ana = d_sz.ana = NULL;
d_x.value = d_y.value = d_z.value = 0.0;
d_sx.value = d_sy.value = d_sz.value = 0.0;
d_x.af = this; d_y.af = this; d_z.af = this;
d_sx.af = this; d_sy.af = this; d_sz.af = this;
//--------------------------------------------------------------------
// Open analog remotes for any channels that have non-NULL names.
// If the name starts with the "*" character, use tracker
// connection rather than getting a new connection for it.
// Set up callbacks to handle updates to the analog values
setup_channel(&d_x);
setup_channel(&d_y);
setup_channel(&d_z);
setup_channel(&d_sx);
setup_channel(&d_sy);
setup_channel(&d_sz);
//--------------------------------------------------------------------
// Open the reset button if is has a non-NULL name.
// If the name starts with the "*" character, use tracker
// connection rather than getting a new connection for it.
// Set up callback for it to reset the matrix to identity.
// If the name is NULL, don't do anything.
if (params->reset_name != NULL) {
// Open the button device and point the remote at it.
// If the name starts with the '*' character, use
// the server connection rather than making a new one.
if (params->reset_name[0] == '*') {
try {
d_reset_button = new vrpn_Button_Remote
(&(params->reset_name[1]),
d_connection);
} catch (...) {
d_reset_button = NULL;
}
} else {
try {
d_reset_button = new vrpn_Button_Remote
(params->reset_name);
} catch (...) {
d_reset_button = NULL;
}
}
if (d_reset_button == NULL) {
fprintf(stderr,"vrpn_Tracker_AnalogFly: "
"Can't open Button %s\n",params->reset_name);
} else {
// Set up the callback handler for the channel
d_reset_button->register_change_handler
(this, handle_reset_press);
}
}
//--------------------------------------------------------------------
// Open the clutch button if is has a non-NULL name.
// If the name starts with the "*" character, use tracker
// connection rather than getting a new connection for it.
// Set up callback for it to control clutching.
// If the name is NULL, don't do anything.
if (params->clutch_name != NULL) {
// Open the button device and point the remote at it.
// If the name starts with the '*' character, use
// the server connection rather than making a new one.
if (params->clutch_name[0] == '*') {
try {
d_clutch_button = new vrpn_Button_Remote
(&(params->clutch_name[1]),
d_connection);
} catch (...) {
d_clutch_button = NULL;
}
} else {
try {
d_clutch_button = new vrpn_Button_Remote
(params->clutch_name);
} catch (...) {
d_clutch_button = NULL;
}
}
if (d_clutch_button == NULL) {
fprintf(stderr,"vrpn_Tracker_AnalogFly: "
"Can't open Button %s\n",params->clutch_name);
} else {
// Set up the callback handler for the channel
d_clutch_button->register_change_handler
(this, handle_clutch_press);
}
}
// If the clutch button is NULL, then engage the clutch always.
if (params->clutch_name == NULL) {
d_clutch_engaged = true;
}
//--------------------------------------------------------------------
// Whenever we get the first connection to this server, we also
// want to reset the matrix to identity, so that you start at the
// beginning. Set up a handler to do this.
register_autodeleted_handler(d_connection->register_message_type(vrpn_got_first_connection),
handle_newConnection, this);
//--------------------------------------------------------------------
// Set the initialization matrix to identity, then also set
// the current matrix to identity and the clutch matrix to
// identity.
for ( i =0; i< 4; i++) {
for (int j=0; j< 4; j++) {
d_initMatrix[i][j] = 0;
}
}
d_initMatrix[0][0] = d_initMatrix[1][1] = d_initMatrix[2][2] =
d_initMatrix[3][3] = 1.0;
reset();
q_matrix_copy(d_clutchMatrix, d_initMatrix);
q_matrix_copy(d_currentMatrix, d_initMatrix);
//--------------------------------------------------------------------
// Set the current timestamp to "now" and current matrix to identity
// for absolute trackers. This is done in case we never hear from the
// analog devices. Reset doesn't do this for absolute trackers.
if (d_absolute) {
vrpn_gettimeofday(&d_prevtime, NULL);
vrpn_Tracker::timestamp = d_prevtime;
q_matrix_copy(d_currentMatrix, d_initMatrix);
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();
}
