vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_AnalogFly.C
Go to the documentation of this file.
1 #include <math.h> // for M_PI, pow, fabs
2 
3 #include "quat.h" // for q_xyz_quat_type, etc
4 #include "vrpn_Connection.h" // for vrpn_Connection, etc
6 
7 #undef VERBOSE
8 
9 #ifndef M_PI
10 #define M_PI 3.14159265358979323846
11 #endif
12 
14  (const char * name, vrpn_Connection * trackercon,
15  vrpn_Tracker_AnalogFlyParam * params, float update_rate,
16  bool absolute, bool reportChanges,
17  bool worldFrame) :
18  vrpn_Tracker (name, trackercon),
19  d_reset_button(NULL),
20  d_which_button (params->reset_which),
21  d_clutch_button(NULL),
22  d_clutch_which (params->clutch_which),
23  d_clutch_engaged(false),
24  d_clutch_was_off(false),
25  d_update_interval (update_rate ? (1/update_rate) : 1.0),
26  d_absolute (absolute),
27  d_reportChanges (reportChanges),
28  d_worldFrame (worldFrame)
29 {
30  int i;
31 
32  d_x.axis = params->x; d_y.axis = params->y; d_z.axis = params->z;
33  d_sx.axis = params->sx; d_sy.axis = params->sy; d_sz.axis = params->sz;
34 
35  d_x.ana = d_y.ana = d_z.ana = NULL;
36  d_sx.ana = d_sy.ana = d_sz.ana = NULL;
37 
38  d_x.value = d_y.value = d_z.value = 0.0;
39  d_sx.value = d_sy.value = d_sz.value = 0.0;
40 
41  d_x.af = this; d_y.af = this; d_z.af = this;
42  d_sx.af = this; d_sy.af = this; d_sz.af = this;
43 
44  //--------------------------------------------------------------------
45  // Open analog remotes for any channels that have non-NULL names.
46  // If the name starts with the "*" character, use tracker
47  // connection rather than getting a new connection for it.
48  // Set up callbacks to handle updates to the analog values
49  setup_channel(&d_x);
50  setup_channel(&d_y);
51  setup_channel(&d_z);
52  setup_channel(&d_sx);
53  setup_channel(&d_sy);
54  setup_channel(&d_sz);
55 
56  //--------------------------------------------------------------------
57  // Open the reset button if is has a non-NULL name.
58  // If the name starts with the "*" character, use tracker
59  // connection rather than getting a new connection for it.
60  // Set up callback for it to reset the matrix to identity.
61 
62  // If the name is NULL, don't do anything.
63  if (params->reset_name != NULL) {
64 
65  // Open the button device and point the remote at it.
66  // If the name starts with the '*' character, use
67  // the server connection rather than making a new one.
68  if (params->reset_name[0] == '*') {
69  d_reset_button = new vrpn_Button_Remote
70  (&(params->reset_name[1]),
71  d_connection);
72  } else {
73  d_reset_button = new vrpn_Button_Remote
74  (params->reset_name);
75  }
76  if (d_reset_button == NULL) {
77  fprintf(stderr,"vrpn_Tracker_AnalogFly: "
78  "Can't open Button %s\n",params->reset_name);
79  } else {
80  // Set up the callback handler for the channel
81  d_reset_button->register_change_handler
82  (this, handle_reset_press);
83  }
84  }
85 
86  //--------------------------------------------------------------------
87  // Open the clutch button if is has a non-NULL name.
88  // If the name starts with the "*" character, use tracker
89  // connection rather than getting a new connection for it.
90  // Set up callback for it to control clutching.
91 
92  // If the name is NULL, don't do anything.
93  if (params->clutch_name != NULL) {
94 
95  // Open the button device and point the remote at it.
96  // If the name starts with the '*' character, use
97  // the server connection rather than making a new one.
98  if (params->clutch_name[0] == '*') {
99  d_clutch_button = new vrpn_Button_Remote
100  (&(params->clutch_name[1]),
101  d_connection);
102  } else {
103  d_clutch_button = new vrpn_Button_Remote
104  (params->clutch_name);
105  }
106  if (d_clutch_button == NULL) {
107  fprintf(stderr,"vrpn_Tracker_AnalogFly: "
108  "Can't open Button %s\n",params->clutch_name);
109  } else {
110  // Set up the callback handler for the channel
111  d_clutch_button->register_change_handler
112  (this, handle_clutch_press);
113  }
114  }
115 
116  // If the clutch button is NULL, then engage the clutch always.
117  if (params->clutch_name == NULL) {
118  d_clutch_engaged = true;
119  }
120 
121  //--------------------------------------------------------------------
122  // Whenever we get the first connection to this server, we also
123  // want to reset the matrix to identity, so that you start at the
124  // beginning. Set up a handler to do this.
125  register_autodeleted_handler(d_connection->register_message_type(vrpn_got_first_connection),
126  handle_newConnection, this);
127 
128  //--------------------------------------------------------------------
129  // Set the initialization matrix to identity, then also set
130  // the current matrix to identity and the clutch matrix to
131  // identity.
132  for ( i =0; i< 4; i++) {
133  for (int j=0; j< 4; j++) {
134  d_initMatrix[i][j] = 0;
135  }
136  }
137 
138  d_initMatrix[0][0] = d_initMatrix[1][1] = d_initMatrix[2][2] =
139  d_initMatrix[3][3] = 1.0;
140  reset();
141  q_matrix_copy(d_clutchMatrix, d_initMatrix);
142  q_matrix_copy(d_currentMatrix, d_initMatrix);
143 
144  //--------------------------------------------------------------------
145  // Set the current timestamp to "now" and current matrix to identity
146  // for absolute trackers. This is done in case we never hear from the
147  // analog devices. Reset doesn't do this for absolute trackers.
148  if (d_absolute) {
149  vrpn_gettimeofday(&d_prevtime, NULL);
150  vrpn_Tracker::timestamp = d_prevtime;
151  q_matrix_copy(d_currentMatrix, d_initMatrix);
152  convert_matrix_to_tracker();
153  }
154 }
155 
157 {
158  // Tear down the analog update callbacks and remotes
165 
166  // Tear down the reset button update callback and remote (if there is one)
167  if (d_reset_button != NULL) {
170  delete d_reset_button;
171  }
172 
173  // Tear down the clutch button update callback and remote (if there is one)
174  if (d_clutch_button != NULL) {
177  delete d_clutch_button;
178  }
179 }
180 
181 // This routine handles updates of the analog values. The value coming in is
182 // adjusted per the parameters in the full axis description, and then used to
183 // update the value there. The value is used by the matrix-generation code in
184 // mainloop() to update the transformations; that work is not done here.
185 
187  (void *userdata, const vrpn_ANALOGCB info)
188 {
189  vrpn_TAF_fullaxis *full = (vrpn_TAF_fullaxis *)userdata;
190  double value = info.channel[full->axis.channel];
191  double value_offset = value - full->axis.offset;
192  double value_abs = fabs(value_offset);
193 
194  // If we're an absolute channel, store the time of the report
195  // into the tracker's timestamp field.
196  if (full->af->d_absolute) {
197  full->af->vrpn_Tracker::timestamp = info.msg_time;
198  }
199 
200  // If we're not above threshold, store zero and we're done!
201  if (value_abs <= full->axis.thresh) {
202  full->value = 0.0;
203  return;
204  }
205 
206  // Scale and apply the power to the value (maintaining its sign)
207  if (value_offset >=0) {
208  full->value = pow(value_offset*full->axis.scale, (double) full->axis.power);
209  } else {
210  full->value = -pow(value_abs*full->axis.scale, (double) full->axis.power);
211  }
212 }
213 
214 // This routine will reset the matrix to identity when the reset button is
215 // pressed.
216 
218  (void *userdata, const vrpn_BUTTONCB info)
219 {
221 
222  // If this is the correct button, and it has just been pressed, then
223  // reset the matrix.
224  if ( (info.button == me->d_which_button) && (info.state == 1) ) {
225  me->reset();
226  }
227 }
228 
229 // This handle state changes associated with the clutch button.
230 
232  (void *userdata, const vrpn_BUTTONCB info)
233 {
235 
236  // If this is the correct button, set the clutch state according to
237  // the value of the button.
238  if (info.button == me->d_clutch_which) {
239  if (info.state == 1) {
240  me->d_clutch_engaged = true;
241  } else {
242  me->d_clutch_engaged = false;
243  }
244  }
245 }
246 
247 // This sets up the Analog Remote for one channel, setting up the callback
248 // needed to adjust the value based on changes in the analog input.
249 // Returns 0 on success and -1 on failure.
250 
252 {
253  // If the name is NULL, we're done.
254  if (full->axis.name == NULL) { return 0; }
255 
256  // Open the analog device and point the remote at it.
257  // If the name starts with the '*' character, use the server
258  // connection rather than making a new one.
259  if (full->axis.name[0] == '*') {
260  full->ana = new vrpn_Analog_Remote(&(full->axis.name[1]),
261  d_connection);
262 #ifdef VERBOSE
263  printf("vrpn_Tracker_AnalogFly: Adding local analog %s\n",
264  &(full->axis.name[1]));
265 #endif
266  } else {
267  full->ana = new vrpn_Analog_Remote(full->axis.name);
268 #ifdef VERBOSE
269  printf("vrpn_Tracker_AnalogFly: Adding remote analog %s\n",
270  full->axis.name);
271 #endif
272  }
273  if (full->ana == NULL) {
274  fprintf(stderr,"vrpn_Tracker_AnalogFly: "
275  "Can't open Analog %s\n",full->axis.name);
276  return -1;
277  }
278 
279  // Set up the callback handler for the channel
280  return full->ana->register_change_handler(full, handle_analog_update);
281 }
282 
283 // This tears down the Analog Remote for one channel, undoing everything that
284 // the setup did. Returns 0 on success and -1 on failure.
285 
287 {
288  int ret;
289 
290  // If the analog pointer is NULL, we're done.
291  if (full->ana == NULL) { return 0; }
292 
293  // Turn off the callback handler for the channel
294  ret = full->ana->unregister_change_handler((void*)full,
296 
297  // Delete the analog device.
298  delete full->ana;
299 
300  return ret;
301 }
302 
303 // static
306 {
307 
308  printf("Get a new connection, reset virtual_Tracker\n");
309  ((vrpn_Tracker_AnalogFly *) userdata)->reset();
310 
311  // Always return 0 here, because nonzero return means that the input data
312  // was garbage, not that there was an error. If we return nonzero from a
313  // vrpn_Connection handler, it shuts down the connection.
314  return 0;
315 }
316 
324 {
325  // Set the clutch matrix to the identity.
326  q_matrix_copy(d_clutchMatrix, d_initMatrix);
327 
328  // Set the matrix back to the identity matrix
329  q_matrix_copy(d_currentMatrix, d_initMatrix);
331 
332  // Convert the matrix into quaternion notation and copy into the
333  // tracker pos and quat elements.
335 }
336 
338 {
339  struct timeval now;
340  double interval; // How long since the last report, in secs
341 
342  // Call generic server mainloop, since we are a server
343  server_mainloop();
344 
345  // Mainloop() all of the analogs that are defined and the button
346  // so that we will get all of the values fresh.
347  if (d_x.ana != NULL) { d_x.ana->mainloop(); };
348  if (d_y.ana != NULL) { d_y.ana->mainloop(); };
349  if (d_z.ana != NULL) { d_z.ana->mainloop(); };
350  if (d_sx.ana != NULL) { d_sx.ana->mainloop(); };
351  if (d_sy.ana != NULL) { d_sy.ana->mainloop(); };
352  if (d_sz.ana != NULL) { d_sz.ana->mainloop(); };
353  if (d_reset_button != NULL) { d_reset_button->mainloop(); };
354  if (d_clutch_button != NULL) { d_clutch_button->mainloop(); };
355 
356  // See if it has been long enough since our last report.
357  // If so, generate a new one.
358  vrpn_gettimeofday(&now, NULL);
359  interval = vrpn_TimevalDurationSeconds(now, d_prevtime);
360 
361  if (shouldReport(interval)) {
362 
363  // Set the time on the report to now, if not an absolute
364  // tracker. Absolute trackers have their time values set
365  // to match the time at which their analog devices gave the
366  // last report.
367  if (!d_absolute) {
369  }
370 
371  // Figure out the new matrix based on the current values and
372  // the length of the interval since the last report
374 
375  // pack and deliver tracker report;
376  if (d_connection) {
377  char msgbuf[1000];
378  int len = encode_to(msgbuf);
380  position_m_id, d_sender_id, msgbuf,
382  fprintf(stderr,"Tracker AnalogFly: "
383  "cannot write message: tossing\n");
384  }
385  } else {
386  fprintf(stderr,"Tracker AnalogFly: "
387  "No valid connection\n");
388  }
389 
390  // We just sent a report, so reset the time
391  d_prevtime = now;
392  }
393 
394  // We're not always sending reports, but we still want to
395  // update the interval clock so that we don't integrate over
396  // too long a timespan when we do finally report a change.
397  if (interval >= d_update_interval) {
398  d_prevtime = now;
399  }
400 }
401 
402 // This routine will update the current matrix based on the current values
403 // in the offsets list for each axis, and the length of time over which the
404 // action is taking place (time_interval).
405 // Handling of non-absolute trackers: It treats the values as either
406 // meters/second or else rotations/second to be integrated over the interval
407 // to adjust the current matrix.
408 // Handling of absolute trackers: It always assumes that the starting matrix
409 // is the identity, so that the values are absolute offsets/rotations.
410 // XXX Later, it would be cool to have non-absolute trackers send velocity
411 // information as well, since it knows what this is.
412 
414  (double time_interval)
415 {
416  double tx,ty,tz, rx,ry,rz; // Translation (m/s) and rotation (rad/sec)
417  q_matrix_type diffM; // Difference (delta) matrix
418 
419  // For absolute trackers, the interval is treated as "1", so that the
420  // translations and rotations are unscaled;
421  if (d_absolute) { time_interval = 1.0; };
422 
423  // compute the translation and rotation
424  tx = d_x.value * time_interval;
425  ty = d_y.value * time_interval;
426  tz = d_z.value * time_interval;
427 
428  rx = d_sx.value * time_interval * (2*M_PI);
429  ry = d_sy.value * time_interval * (2*M_PI);
430  rz = d_sz.value * time_interval * (2*M_PI);
431 
432  // Build a rotation matrix, then add in the translation
433  q_euler_to_col_matrix(diffM, rz, ry, rx);
434  diffM[3][0] = tx; diffM[3][1] = ty; diffM[3][2] = tz;
435 
436  // While the clutch is not engaged, we don't move. Record that
437  // the clutch was off so that we know later when it is re-engaged.
438  if (!d_clutch_engaged) {
439  d_clutch_was_off = true;
440  return;
441  }
442 
443  // When the clutch becomes re-engaged, we store the current matrix
444  // multiplied by the inverse of the present differential matrix so that
445  // the first frame of the mouse-hold leaves us in the same location.
446  // For the absolute matrix, this re-engages new motion at the previous
447  // location.
448  if (d_clutch_engaged && d_clutch_was_off) {
449  d_clutch_was_off = false;
450  q_type diff_orient;
451  // This is backwards, because Euler angles have rotation about Z first...
452  q_from_euler(diff_orient, rz, ry, rx);
453  q_xyz_quat_type diff;
454  q_vec_set(diff.xyz, tx, ty, tz);
455  q_copy(diff.quat, diff_orient);
456  q_xyz_quat_type diff_inverse;
457  q_xyz_quat_invert(&diff_inverse, &diff);
458  q_matrix_type di_matrix;
459  q_to_col_matrix(di_matrix, diff_inverse.quat);
460  di_matrix[3][0] = diff_inverse.xyz[0];
461  di_matrix[3][1] = diff_inverse.xyz[1];
462  di_matrix[3][2] = diff_inverse.xyz[2];
463  q_matrix_mult(d_clutchMatrix, di_matrix, d_currentMatrix);
464  }
465 
466  // Apply the matrix.
467  if (d_absolute) {
468  // The difference matrix IS the current matrix. Catenate it
469  // onto the clutch matrix. If there is no clutching happening,
470  // this matrix will always be the identity so this will just
471  // copy the difference matrix.
472  q_matrix_mult(d_currentMatrix, diffM, d_clutchMatrix);
473  } else {
474  // Multiply the current matrix by the difference matrix to update
475  // it to the current time.
476  if (d_worldFrame) {
477  // If using world frame:
478  // 1. Separate out the translation and add to the differential translation
479  tx += d_currentMatrix[3][0];
480  ty += d_currentMatrix[3][1];
481  tz += d_currentMatrix[3][2];
482  diffM[3][0] = 0; diffM[3][1] = 0; diffM[3][2] = 0;
483  d_currentMatrix[3][0] = 0; d_currentMatrix[3][1] = 0; d_currentMatrix[3][2] = 0;
484 
485  // 2. Compose the rotations.
486  q_matrix_mult(d_currentMatrix, d_currentMatrix, diffM);
487 
488  // 3. Put the new translation back in the matrix.
489  d_currentMatrix[3][0] = tx; d_currentMatrix[3][1] = ty; d_currentMatrix[3][2] = tz;
490 
491  } else {
492  q_matrix_mult(d_currentMatrix, diffM, d_currentMatrix);
493  }
494  }
495 
496  // Finally, convert the matrix into a pos/quat
497  // and copy it into the tracker position and quaternion structures.
498  convert_matrix_to_tracker();
499 }
500 
502 {
503  q_xyz_quat_type xq;
504  int i;
505 
506  q_row_matrix_to_xyz_quat( & xq, d_currentMatrix);
507 
508  for (i=0; i< 3; i++) {
509  pos[i] = xq.xyz[i]; // position;
510  }
511  for (i=0; i< 4; i++) {
512  d_quat[i] = xq.quat[i]; // orientation.
513  }
514 }
515 
517  (double elapsedInterval) const {
518 
519  // If we haven't had enough time pass yet, don't report.
520  if (elapsedInterval < d_update_interval) {
521  return VRPN_FALSE;
522  }
523 
524  // If we're sending a report every interval, regardless of
525  // whether or not there are changes, then send one now.
526  if (!d_reportChanges) {
527  return VRPN_TRUE;
528  }
529 
530  // If anything's nonzero, send the report.
531  // HACK: This values may be unstable, depending on device characteristics;
532  // we may need to designate a small dead zone around zero and only report
533  // if the value is outside the dead zone.
534  if (d_x.value || d_y.value || d_z.value ||
535  d_sx.value || d_sy.value || d_sz.value) {
536  return VRPN_TRUE;
537  }
538 
539  // Enough time has elapsed, but nothing has changed, so return false.
540  return VRPN_FALSE;
541 }
542 
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
This class will turn an analog device such as a joystick or a camera.
bool shouldReport(double elapsedInterval) const
#define M_PI
static int VRPN_CALLBACK handle_newConnection(void *, vrpn_HANDLERPARAM)
int teardown_channel(vrpn_TAF_fullaxis *full)
vrpn_Button_Remote * d_reset_button
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
vrpn_Tracker_AnalogFly(const char *name, vrpn_Connection *trackercon, vrpn_Tracker_AnalogFlyParam *params, float update_rate, bool absolute=vrpn_FALSE, bool reportChanges=VRPN_FALSE, bool worldFrame=VRPN_FALSE)
char * clutch_name
Clutch device that is used to enable relative motion over.
vrpn_TAF_axis sx
Rotation in the positive direction about the three axes.
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
void update_matrix_based_on_values(double time_interval)
Generic connection class not specific to the transport mechanism.
static void VRPN_CALLBACK handle_clutch_press(void *userdata, const vrpn_BUTTONCB info)
char * reset_name
Button device that is used to reset the matrix to the origin.
#define VRPN_CALLBACK
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_TAF_axis x
Translation along each of these three 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...
struct timeval msg_time
Definition: vrpn_Analog.h:169
static void VRPN_CALLBACK handle_reset_press(void *userdata, const vrpn_BUTTONCB info)
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:171
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:533
virtual int register_change_handler(void *userdata, vrpn_ANALOGCHANGEHANDLER handler)
Definition: vrpn_Analog.h:192
virtual void reset(void)
Reset the current matrix to zero and store it into the tracker position/quaternion location...
vrpn_int32 state
Definition: vrpn_Button.h:228
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
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
vrpn_Button_Remote * d_clutch_button
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
vrpn_Analog_Remote * ana
vrpn_Tracker_AnalogFly * af
int setup_channel(vrpn_TAF_fullaxis *full)
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
static void VRPN_CALLBACK handle_analog_update(void *userdata, const vrpn_ANALOGCB info)
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80