10 #define M_PI 3.14159265358979323846 16 bool absolute,
bool reportChanges,
21 d_clutch_button(NULL),
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)
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;
35 d_x.ana = d_y.ana = d_z.ana = NULL;
36 d_sx.ana = d_sy.ana = d_sz.ana = NULL;
38 d_x.value = d_y.value = d_z.value = 0.0;
39 d_sx.value = d_sy.value = d_sz.value = 0.0;
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;
76 if (d_reset_button == NULL) {
77 fprintf(stderr,
"vrpn_Tracker_AnalogFly: " 81 d_reset_button->register_change_handler
82 (
this, handle_reset_press);
106 if (d_clutch_button == NULL) {
107 fprintf(stderr,
"vrpn_Tracker_AnalogFly: " 111 d_clutch_button->register_change_handler
112 (
this, handle_clutch_press);
118 d_clutch_engaged =
true;
126 handle_newConnection,
this);
132 for ( i =0; i< 4; i++) {
133 for (
int j=0; j< 4; j++) {
134 d_initMatrix[i][j] = 0;
138 d_initMatrix[0][0] = d_initMatrix[1][1] = d_initMatrix[2][2] =
139 d_initMatrix[3][3] = 1.0;
141 q_matrix_copy(d_clutchMatrix, d_initMatrix);
142 q_matrix_copy(d_currentMatrix, d_initMatrix);
151 q_matrix_copy(d_currentMatrix, d_initMatrix);
152 convert_matrix_to_tracker();
191 double value_offset = value - full->
axis.
offset;
192 double value_abs = fabs(value_offset);
197 full->
af->vrpn_Tracker::timestamp = info.
msg_time;
201 if (value_abs <= full->axis.thresh) {
207 if (value_offset >=0) {
239 if (info.
state == 1) {
254 if (full->
axis.
name == NULL) {
return 0; }
263 printf(
"vrpn_Tracker_AnalogFly: Adding local analog %s\n",
269 printf(
"vrpn_Tracker_AnalogFly: Adding remote analog %s\n",
273 if (full->
ana == NULL) {
274 fprintf(stderr,
"vrpn_Tracker_AnalogFly: " 275 "Can't open Analog %s\n",full->
axis.
name);
291 if (full->
ana == NULL) {
return 0; }
308 printf(
"Get a new connection, reset virtual_Tracker\n");
382 fprintf(stderr,
"Tracker AnalogFly: " 383 "cannot write message: tossing\n");
386 fprintf(stderr,
"Tracker AnalogFly: " 387 "No valid connection\n");
414 (
double time_interval)
416 double tx,ty,tz, rx,ry,rz;
421 if (d_absolute) { time_interval = 1.0; };
424 tx = d_x.value * time_interval;
425 ty = d_y.value * time_interval;
426 tz = d_z.value * time_interval;
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);
433 q_euler_to_col_matrix(diffM, rz, ry, rx);
434 diffM[3][0] = tx; diffM[3][1] = ty; diffM[3][2] = tz;
438 if (!d_clutch_engaged) {
439 d_clutch_was_off =
true;
448 if (d_clutch_engaged && d_clutch_was_off) {
449 d_clutch_was_off =
false;
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);
472 q_matrix_mult(d_currentMatrix, diffM, d_clutchMatrix);
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;
486 q_matrix_mult(d_currentMatrix, d_currentMatrix, diffM);
489 d_currentMatrix[3][0] = tx; d_currentMatrix[3][1] = ty; d_currentMatrix[3][2] = tz;
492 q_matrix_mult(d_currentMatrix, diffM, d_currentMatrix);
498 convert_matrix_to_tracker();
508 for (i=0; i< 3; i++) {
511 for (i=0; i< 4; i++) {
517 (
double elapsedInterval)
const {
520 if (elapsedInterval < d_update_interval) {
526 if (!d_reportChanges) {
534 if (d_x.value || d_y.value || d_z.value ||
535 d_sx.value || d_sy.value || d_sz.value) {
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...
q_matrix_type d_clutchMatrix
const char * vrpn_got_first_connection
These are the strings that define the system-generated message types that tell when connections are r...
This class will turn an analog device such as a joystick or a camera.
bool shouldReport(double elapsedInterval) const
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.
q_matrix_type d_currentMatrix
vrpn_TAF_axis sx
Rotation in the positive direction about the three axes.
virtual ~vrpn_Tracker_AnalogFly(void)
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.
struct timeval d_prevtime
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
vrpn_TAF_axis x
Translation along each of these three axes.
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...
static void VRPN_CALLBACK handle_reset_press(void *userdata, const vrpn_BUTTONCB info)
vrpn_float64 channel[vrpn_CHANNEL_MAX]
virtual int encode_to(char *buf)
virtual int register_change_handler(void *userdata, vrpn_ANALOGCHANGEHANDLER handler)
virtual void reset(void)
Reset the current matrix to zero and store it into the tracker position/quaternion location.
q_matrix_type d_initMatrix
void convert_matrix_to_tracker(void)
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 ...
#define vrpn_gettimeofday
vrpn_Button_Remote * d_clutch_button
vrpn_int32 d_sender_id
Sender ID registered with the connection.
vrpn_Tracker_AnalogFly * af
int setup_channel(vrpn_TAF_fullaxis *full)
static void VRPN_CALLBACK handle_analog_update(void *userdata, const vrpn_ANALOGCB info)