void vrpn_Tracker_AnalogFly::reset (void)
{
// Set the clutch matrix to the identity.
q_matrix_copy(d_clutchMatrix, d_initMatrix);
// 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::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.
}
}
vrpn_Tracker_JoyFly::vrpn_Tracker_JoyFly
(const char * name, vrpn_Connection * c,
const char * source, const char * set_config,
vrpn_Connection * sourceConnection) :
vrpn_Tracker (name, c) {
int i;
joy_remote = new vrpn_Analog_Remote (source, sourceConnection);
joy_remote->register_change_handler(this, handle_joystick);
c->register_handler(c->register_message_type(vrpn_got_connection),
handle_newConnection, this);
FILE * fp = fopen(set_config, "r");
if (fp == NULL) {
fprintf(stderr, "Can't open joy config file, using default..\n");
fprintf(stderr, "Channel Accel 1.0, Power 1, Init Identity Matrix. \n");
for (i=0; i< 7; i++) {
chanAccel[i] = 1.0;
chanPower[i] = 1;
}
for ( i =0; i< 4; i++)
for (int j=0; j< 4; j++)
initMatrix[i][j] = 0;
initMatrix[0][0] = initMatrix[2][2] =
initMatrix[1][1] = initMatrix[3][3] = 1.0;
} else {
for (i=0; i< 7; i++) {
if (fscanf(fp, "%lf %d", &chanAccel[i], &chanPower[i]) != 2) {
fprintf(stderr,"Cannot read acceleration and power from file\n");
return;
}
fprintf(stderr, "Chan[%d] = (%lf %d)\n", i, chanAccel[i], chanPower[i]);
}
for (i =0; i< 4; i++)
for (int j=0; j< 4; j++) {
if (fscanf(fp, "%lf", &initMatrix[i][j]) < 0) {
perror("vrpn_Tracker_JoyFly::vrpn_Tracker_JoyFly(): Could not read matrix value");
return;
}
}
fclose(fp);
}
q_matrix_copy(currentMatrix, initMatrix);
}
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();
}
}
