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