vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_ButtonFly.C
Go to the documentation of this file.
1 //XXX Send velocity and rotational velocity reports.
2 
3 #include <math.h> // for pow, M_PI, fabs
4 
5 #include "quat.h" // for q_matrix_copy, etc
6 #include "vrpn_Connection.h" // for vrpn_Connection, etc
8 
9 #undef VERBOSE
10 
11 #ifndef M_PI
12 #define M_PI 3.14159265358979323846
13 #endif
14 
16  (const char * name, vrpn_Connection * trackercon,
17  vrpn_Tracker_ButtonFlyParam * params, float update_rate,
18  bool reportChanges) :
19  vrpn_Tracker (name, trackercon),
20  d_vel_scale(NULL),
21  d_vel_scale_value(1.0),
22  d_rot_scale(NULL),
23  d_rot_scale_value(1.0),
24  d_update_interval (update_rate ? (1/update_rate) : 1.0),
25  d_reportChanges (reportChanges)
26 {
27  int i;
28 
29  //--------------------------------------------------------------------
30  // Copy the parameter values and initialize the values and pointers
31  // in the list of axes. The setup_channel() call opens the button
32  // remote and sets up a callback to handle the changes.
33  d_num_axes = params->num_axes;
34  for (i = 0; i < params->num_axes; i++) {
35  d_axes[i].axis = params->axes[i];
36  d_axes[i].active = false;
37  d_axes[i].bf = this;
38  setup_channel(&d_axes[i]);
39  }
40 
41  //--------------------------------------------------------------------
42  // Open the scale analogs if they have non-NULL, non-empty names.
43  // If the name starts with the "*" character, use tracker
44  // connection rather than getting a new connection for it.
45  // Set up a callback for each to set the scale factor.
46 
47  if (params->vel_scale_name[0] != '\0') {
48 
49  // Copy the parameters into our member variables
50  d_vel_scale_channel = params->vel_scale_channel;
51  d_vel_scale_offset = params->vel_scale_offset;
52  d_vel_scale_scale = params->vel_scale_scale;
53  d_vel_scale_power = params->vel_scale_power;
54 
55  // Open the analog device and point the remote at it.
56  // If the name starts with the '*' character, use
57  // the server connection rather than making a new one.
58  if (params->vel_scale_name[0] == '*') {
59  d_vel_scale = new vrpn_Analog_Remote
60  (&(params->vel_scale_name[1]), d_connection);
61  } else {
62  d_vel_scale = new vrpn_Analog_Remote(params->vel_scale_name);
63  }
64 
65  // Set up the callback handler
66  if (d_vel_scale == NULL) {
67  fprintf(stderr,"vrpn_Tracker_ButtonFly: "
68  "Can't open Analog %s\n",params->vel_scale_name);
69  } else {
70  // Set up the callback handler for the channel
71  d_vel_scale->register_change_handler(this, handle_velocity_update);
72  }
73  }
74 
75  if (params->rot_scale_name[0] != '\0') {
76 
77  // Copy the parameters into our member variables
78  d_rot_scale_channel = params->rot_scale_channel;
79  d_rot_scale_offset = params->rot_scale_offset;
80  d_rot_scale_scale = params->rot_scale_scale;
81  d_rot_scale_power = params->rot_scale_power;
82 
83  // Open the analog device and point the remote at it.
84  // If the name starts with the '*' character, use
85  // the server connection rather than making a new one.
86  if (params->rot_scale_name[0] == '*') {
87  d_rot_scale = new vrpn_Analog_Remote
88  (&(params->rot_scale_name[1]), d_connection);
89  } else {
90  d_rot_scale = new vrpn_Analog_Remote(params->rot_scale_name);
91  }
92 
93  // Set up the callback handler
94  if (d_rot_scale == NULL) {
95  fprintf(stderr,"vrpn_Tracker_ButtonFly: "
96  "Can't open Analog %s\n",params->rot_scale_name);
97  } else {
98  // Set up the callback handler for the channel
99  d_rot_scale->register_change_handler(this, handle_rotation_update);
100  }
101  }
102 
103  //--------------------------------------------------------------------
104  // Whenever we get the first connection to this server, we also
105  // want to reset the matrix to identity, so that you start at the
106  // beginning. Set up a handler to do this.
107  register_autodeleted_handler(d_connection->register_message_type
109  handle_newConnection, this);
110 
111  //--------------------------------------------------------------------
112  // Set the initialization matrix to identity, then also set
113  // the current matrix to identity.
114  for ( i =0; i< 4; i++)
115  for (int j=0; j< 4; j++)
116  d_initMatrix[i][j] = 0;
117 
118  d_initMatrix[0][0] = d_initMatrix[1][1] = d_initMatrix[2][2] =
119  d_initMatrix[3][3] = 1.0;
120  reset();
121 }
122 
124 {
125  int i;
126 
127  // Tear down the button axes (which will include the
128  // button callbacks and remotes).
129  for (i = 0; i < d_num_axes; i++) {
131  }
132 
133  // Tear down the analog update callbacks and remotes (if they exist)
134  if (d_vel_scale != NULL) {
136  delete d_vel_scale;
137  }
138  if (d_rot_scale != NULL) {
140  delete d_rot_scale;
141  }
142 }
143 
144 // This sets up the Button Remote for one channel, setting up the callback
145 // needed to adjust the value based on changes in the button.
146 // Returns 0 on success and -1 on failure.
147 
149 {
150  // Open the button device and point the remote at it.
151  // If the name starts with the '*' character, use the server
152  // connection rather than making a new one.
153  if (full->axis.name[0] == '*') {
154  full->btn = new vrpn_Button_Remote(&(full->axis.name[1]),
155  d_connection);
156 #ifdef VERBOSE
157  printf("vrpn_Tracker_ButtonFly: Adding local button %s\n",
158  &(full->axis.name[1]));
159 #endif
160  } else {
161  full->btn = new vrpn_Button_Remote(full->axis.name);
162 #ifdef VERBOSE
163  printf("vrpn_Tracker_ButtonFly: Adding remote button %s\n",
164  full->axis.name);
165 #endif
166  }
167  if (full->btn == NULL) {
168  fprintf(stderr,"vrpn_Tracker_ButtonFly: "
169  "Can't open Button %s\n",full->axis.name);
170  return -1;
171  }
172 
173  // Set up the callback handler for the channel
174  return full->btn->register_change_handler(full, handle_button_update);
175 }
176 
177 // This tears down the Button Remote for one channel, undoing everything that
178 // the setup did. Returns 0 on success and -1 on failure.
179 
181 {
182  int ret;
183 
184  // If the button pointer is NULL, we're done.
185  if (full->btn == NULL) { return 0; }
186 
187  // Turn off the callback handler for the channel
188  ret = full->btn->unregister_change_handler((void*)full, handle_button_update);
189 
190  // Delete the analog device and point the remote at it.
191  delete full->btn;
192 
193  return ret;
194 }
195 
196 // This routine handles updates of the velocity-scale value. The value coming in is
197 // adjusted per the parameters in the member variables, and then used to
198 // update the value there. The value is used by the matrix-generation code in
199 // mainloop() to update the transformations; that work is not done here.
200 
202  (void *userdata, const vrpn_ANALOGCB info)
203 {
205  double value = info.channel[me->d_vel_scale_channel];
206  double value_offset = value - me->d_vel_scale_offset;
207  double value_scaled = value_offset * me->d_vel_scale_scale;
208  double value_abs = fabs(value_scaled);
209  double value_powered;
210 
211  // Scale and apply the power to the value (maintaining its sign)
212  if (value_offset >=0) {
213  value_powered = pow(value_abs, (double) me->d_vel_scale_power);
214  } else {
215  value_powered = -pow(value_abs, (double) me->d_vel_scale_power);
216  }
217 
218  // Store the value for use by the matrix code
219  me->d_vel_scale_value = (float)value_powered;
220 }
221 
222 // This routine handles updates of the rotational-scale value. The value coming in is
223 // adjusted per the parameters in the member variables, and then used to
224 // update the value there. The value is used by the matrix-generation code in
225 // mainloop() to update the transformations; that work is not done here.
226 
228  (void *userdata, const vrpn_ANALOGCB info)
229 {
231  double value = info.channel[me->d_rot_scale_channel];
232  double value_offset = value - me->d_rot_scale_offset;
233  double value_scaled = value_offset * me->d_rot_scale_scale;
234  double value_abs = fabs(value_scaled);
235  double value_powered;
236 
237  // Scale and apply the power to the value (maintaining its sign)
238  if (value_offset >=0) {
239  value_powered = pow(value_abs, (double) me->d_rot_scale_power);
240  } else {
241  value_powered = -pow(value_abs, (double) me->d_rot_scale_power);
242  }
243 
244  // Store the value for use by the matrix code
245  me->d_rot_scale_value = (float)value_powered;
246 }
247 
248 // This routine will handle a button being pressed. For absolute
249 // buttons, it stores the value directly into the matrix when the
250 // button is pressed. For differential channels, it adds marks
251 // the channel as active so that its difference will be included
252 // the the total difference computation.
253 
255  (void *userdata, const vrpn_BUTTONCB info)
256 {
257  vrpn_TBF_fullaxis *axis = (vrpn_TBF_fullaxis*)userdata;
258 
259  // If this is not the correct button for this axis, return
260  if (axis->axis.channel != info.button) {
261  return;
262  }
263 
264  // If this is an absolute axis, and the button has just been pressed,
265  // then set the matrix to the one for this button.
266  if (axis->axis.absolute) {
267  if (info.state == 1) {
268  double tx,ty,tz, rx,ry,rz; //< Translation and rotation to set to
269  q_matrix_type newMatrix; //< Matrix set to these values.
270 
271  // compute the translation and rotation
272  tx = axis->axis.vec[0];
273  ty = axis->axis.vec[1];
274  tz = axis->axis.vec[2];
275 
276  rx = axis->axis.rot[0] * (2*M_PI);
277  ry = axis->axis.rot[1] * (2*M_PI);
278  rz = axis->axis.rot[2] * (2*M_PI);
279 
280  // Build a rotation matrix, then add in the translation
281  q_euler_to_col_matrix(newMatrix, rz, ry, rx);
282  newMatrix[3][0] = tx; newMatrix[3][1] = ty; newMatrix[3][2] = tz;
283 
284  // Copy the new matrix to the current matrix
285  // and then update the tracker based on it
286  q_matrix_copy(axis->bf->d_currentMatrix, newMatrix);
287  axis->bf->convert_matrix_to_tracker();
288 
289  // Mark the axis as active, so that a report will be generated
290  // next time. For absolute channels, this is marked inactive when
291  // the report based on it is generated.
292  axis->active = true;
293  }
294 
295  // This is a differential axis, so mark it as active or not
296  // depending on whether the button was pressed or not.
297  } else {
298  axis->active = (info.state != 0);
299 
300  // XXX To be strictly correct, we should record the time at which
301  // the activity started so that it can take effect between update
302  // intervals.
303  }
304 }
305 
306 // static
309 {
310 
311  printf("Get a new connection, reset virtual_Tracker\n");
312  ((vrpn_Tracker_ButtonFly *) userdata)->reset();
313 
314  // Always return 0 here, because nonzero return means that the input data
315  // was garbage, not that there was an error. If we return nonzero from a
316  // vrpn_Connection handler, it shuts down the connection.
317  return 0;
318 }
319 
325 {
326  // Set the matrix back to the identity matrix
327  q_matrix_copy(d_currentMatrix, d_initMatrix);
329 
330  // Convert the matrix into quaternion notation and copy into the
331  // tracker pos and quat elements.
333 }
334 
336 {
337  int i;
338  struct timeval now;
339  double interval; // How long since the last report, in secs
340 
341  // Call generic server mainloop, since we are a server
342  server_mainloop();
343 
344  // Mainloop() all of the buttons that are defined and the analog
345  // scale values if they are defined so that we will get all of
346  // the values fresh.
347  for (i = 0; i < d_num_axes; i++) {
348  d_axes[i].btn->mainloop();
349  }
350  if (d_vel_scale != NULL) { d_vel_scale->mainloop(); };
351  if (d_rot_scale != NULL) { d_rot_scale->mainloop(); };
352 
353  // See if it has been long enough since our last report.
354  // If so, generate a new one.
355  vrpn_gettimeofday(&now, NULL);
356  interval = vrpn_TimevalDurationSeconds(now, d_prevtime);
357 
358  if (shouldReport(interval)) {
359  // Figure out the new matrix based on the current values and
360  // the length of the interval since the last report
362  d_prevtime = now;
363 
364  // Set the time on the report to now.
366 
367  // pack and deliver tracker report;
368  if (d_connection) {
369  char msgbuf[1000];
370  int len = encode_to(msgbuf);
372  position_m_id, d_sender_id, msgbuf,
374  fprintf(stderr,"Tracker ButtonFly: cannot write message: tossing\n");
375  }
376  } else {
377  fprintf(stderr,"Tracker ButtonFly: No valid connection\n");
378  }
379 
380  // We're not always sending reports, but we still want to
381  // update the matrix so that we don't integrate over
382  // too long a timespan when we do finally report a change.
383  } else if (interval >= d_update_interval) {
384  // Figure out the new matrix based on the current values and
385  // the length of the interval since the last report
387  d_prevtime = now;
388  }
389 }
390 
391 // This routine will update the current matrix based on the current values
392 // in the offsets list for each axis, and the length of time over which the
393 // action is taking place (time_interval).
394 // Handling of non-absolute trackers: It treats the values as either
395 // meters/second or else rotations/second to be integrated over the interval
396 // to adjust the current matrix. All active differential axis are added
397 // together, and then the results are scaled by the velocity and rotational
398 // analog scale factors.
399 // Handling of absolute trackers: Their effects are handled in the button
400 // callback and in the shouldReport methods.
401 // XXX Later, it would be cool to have non-absolute trackers send velocity
402 // information as well, since it knows what this is.
403 
405  (double time_interval)
406 {
407  double tx,ty,tz, rx,ry,rz; // Translation (m/s) and rotation (rad/sec)
408  q_matrix_type diffM; // Difference (delta) matrix
409  int i;
410 
411  // Set the translation and rotation to zero, then go through all of the
412  // axis and sum in the contributions from any that are differential and
413  // active.
414  tx = ty = tz = rx = ry = rz = 0.0;
415 
416  for (i = 0; i < d_num_axes; i++) {
417  if (d_axes[i].active && !d_axes[i].axis.absolute) {
418  tx += d_axes[i].axis.vec[0] * time_interval * d_vel_scale_value;
419  ty += d_axes[i].axis.vec[1] * time_interval * d_vel_scale_value;
420  tz += d_axes[i].axis.vec[2] * time_interval * d_vel_scale_value;
421 
422  rx = d_axes[i].axis.rot[0] * time_interval * (2*M_PI) * d_rot_scale_value;
423  ry = d_axes[i].axis.rot[1] * time_interval * (2*M_PI) * d_rot_scale_value;
424  rz = d_axes[i].axis.rot[2] * time_interval * (2*M_PI) * d_rot_scale_value;
425  }
426  }
427 
428  // Build a rotation matrix, then add in the translation
429  q_euler_to_col_matrix(diffM, rz, ry, rx);
430  diffM[3][0] = tx; diffM[3][1] = ty; diffM[3][2] = tz;
431 
432  // Multiply the current matrix by the difference matrix to update
433  // it to the current time. Then convert the matrix into a pos/quat
434  // and copy it into the tracker position and quaternion structures.
435  q_matrix_type final;
436  q_matrix_mult(final, diffM, d_currentMatrix);
437  q_matrix_copy(d_currentMatrix, final);
438  convert_matrix_to_tracker();
439 }
440 
442 {
443  q_xyz_quat_type xq;
444  int i;
445 
446  q_row_matrix_to_xyz_quat( & xq, d_currentMatrix);
447 
448  for (i=0; i< 3; i++) {
449  pos[i] = xq.xyz[i]; // position;
450  }
451  for (i=0; i< 4; i++) {
452  d_quat[i] = xq.quat[i]; // orientation.
453  }
454 }
455 
457  (double elapsedInterval) {
458  int i;
459  bool found_any;
460 
461  // If we come across an absolute channel that is active,
462  // we'll want to report. We also want to reset that channel's
463  // activity to false. Go through the whole list to make sure
464  // we handle all of them at once.
465  found_any = false;
466  for (i = 0; i < d_num_axes; i++) {
467  if (d_axes[i].active && d_axes[i].axis.absolute) {
468  found_any = true;
469  d_axes[i].active = false;
470  }
471  }
472  if (found_any) {
473  return VRPN_TRUE;
474  }
475 
476  // If we haven't had enough time pass yet, don't report.
477  if (elapsedInterval < d_update_interval) {
478  return VRPN_FALSE;
479  }
480 
481  // If we're sending a report every interval, regardless of
482  // whether or not there are changes, then send one now.
483  if (!d_reportChanges) {
484  return VRPN_TRUE;
485  }
486 
487  // If any differential channels are active, send the report.
488  found_any = false;
489  for (i = 0; i < d_num_axes; i++) {
490  if (d_axes[i].active && !d_axes[i].axis.absolute) {
491  found_any = true;
492  }
493  }
494  if (found_any) {
495  return VRPN_TRUE;
496  }
497 
498  // Enough time has elapsed, but nothing has changed, so return false.
499  return VRPN_FALSE;
500 }
501 
static void VRPN_CALLBACK handle_rotation_update(void *userdata, const vrpn_ANALOGCB info)
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
const char * vrpn_got_first_connection
These are the strings that define the system-generated message types that tell when connections are r...
vrpn_int32 button
Definition: vrpn_Button.h:227
char vel_scale_name[200]
Analog device that scales the translation.
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
void update_matrix_based_on_values(double time_interval)
static void VRPN_CALLBACK handle_velocity_update(void *userdata, const vrpn_ANALOGCB info)
vrpn_Tracker_ButtonFly(const char *name, vrpn_Connection *trackercon, vrpn_Tracker_ButtonFlyParam *params, float update_rate, bool reportChanges=VRPN_FALSE)
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
This class will turn a button device into a tracker by interpreting.
Generic connection class not specific to the transport mechanism.
bool shouldReport(double elapsedInterval)
virtual int register_change_handler(void *userdata, vrpn_BUTTONCHANGEHANDLER handler)
Definition: vrpn_Button.h:262
vrpn_Tracker_ButtonFly * bf
int setup_channel(vrpn_TBF_fullaxis *full)
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
Definition: vrpn_Shared.C:135
vrpn_TBF_axis axes[vrpn_BUTTONFLY_MAXAXES]
List of buttons that control axes.
virtual int unregister_change_handler(void *userdata, vrpn_BUTTONCHANGEHANDLER handler)
Definition: vrpn_Button.h:267
vrpn_Connection * d_connection
Connection that this object talks to.
This structure is what is passed to a vrpn_Connection message callback.
virtual int pack_message(vrpn_uint32 len, struct timeval time, vrpn_int32 type, vrpn_int32 sender, const char *buffer, vrpn_uint32 class_of_service)
Pack a message that will be sent the next time mainloop() is called. Turn off the RELIABLE flag if yo...
q_matrix_type d_initMatrix
Initial, current, and velocity matrices for the tracker.
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:171
vrpn_Analog_Remote * d_vel_scale
Analog device that scales the translation.
#define M_PI
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:533
vrpn_Button_Remote * btn
vrpn_int32 state
Definition: vrpn_Button.h:228
static void VRPN_CALLBACK handle_button_update(void *userdata, const vrpn_BUTTONCB info)
static int VRPN_CALLBACK handle_newConnection(void *, vrpn_HANDLERPARAM)
virtual int unregister_change_handler(void *userdata, vrpn_ANALOGCHANGEHANDLER handler)
Definition: vrpn_Analog.h:197
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Definition: vrpn_Analog.C:328
vrpn_TBF_fullaxis d_axes[vrpn_BUTTONFLY_MAXAXES]
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Definition: vrpn_Button.C:958
vrpn_int32 d_sender_id
Sender ID registered with the connection.
struct timeval timestamp
Definition: vrpn_Tracker.h:100
char rot_scale_name[200]
Analog device that scales the rotation.
vrpn_Analog_Remote * d_rot_scale
Analog device that scales the rotation.
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
virtual void reset(void)
Reset the current matrix to zero and store it into the tracker position/quaternion location.
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80
int teardown_channel(vrpn_TBF_fullaxis *full)