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