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),
19 d_vel_scale_value(1.0),
20 d_rot_scale(NULL),
21 d_rot_scale_value(1.0)
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
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 {
57 (&(params->vel_scale_name[1]), d_connection);
58 } catch (...) { d_vel_scale = NULL; }
59 } else {
60 try {
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
72 }
73 }
74
75 if (params->rot_scale_name[0] != '\0') {
76
77 // Copy the parameters into our member variables
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 {
89 (&(params->rot_scale_name[1]), d_connection);
90 } catch (...) { d_rot_scale = NULL; }
91 } else {
92 try {
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
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.
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) {
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) {
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
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);
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
virtual int unregister_change_handler(void *userdata, vrpn_ANALOGCHANGEHANDLER handler)
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
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...
virtual int register_change_handler(void *userdata, vrpn_BUTTONCHANGEHANDLER handler)
virtual int unregister_change_handler(void *userdata, vrpn_BUTTONCHANGEHANDLER handler)
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Generic connection class not specific to the transport mechanism.
virtual vrpn_int32 register_message_type(const char *name)
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...
vrpn_Button_Remote * btn
vrpn_Tracker_ButtonFly * bf
char rot_scale_name[200]
Analog device that scales the rotation.
vrpn_TBF_axis axes[vrpn_BUTTONFLY_MAXAXES]
List of buttons that control axes.
char vel_scale_name[200]
Analog device that scales the translation.
This class will turn a button device into a tracker by interpreting.
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_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