13 const char * source,
const char * set_config,
20 joy_remote->register_change_handler(
this, handle_joystick);
22 handle_newConnection,
this);
24 FILE * fp = fopen(set_config,
"r");
26 fprintf(stderr,
"Can't open joy config file, using default..\n");
27 fprintf(stderr,
"Channel Accel 1.0, Power 1, Init Identity Matrix. \n");
28 for (i=0; i< 7; i++) {
32 for ( i =0; i< 4; i++)
33 for (
int j=0; j< 4; j++)
35 initMatrix[0][0] = initMatrix[2][2] =
36 initMatrix[1][1] = initMatrix[3][3] = 1.0;
38 for (i=0; i< 7; i++) {
39 if (fscanf(fp,
"%lf %d", &chanAccel[i], &chanPower[i]) != 2) {
40 fprintf(stderr,
"Cannot read acceleration and power from file\n");
43 fprintf(stderr,
"Chan[%d] = (%lf %d)\n", i, chanAccel[i], chanPower[i]);
46 for (
int j=0; j< 4; j++) {
47 if (fscanf(fp,
"%lf", &initMatrix[i][j]) < 0) {
48 perror(
"vrpn_Tracker_JoyFly::vrpn_Tracker_JoyFly(): Could not read matrix value");
55 q_matrix_copy(currentMatrix, initMatrix);
68 if (joy_remote != NULL)
72 fprintf(stderr,
"Sending a report\n");
80 "\nvrpn_Tracker_Flock: cannot write message ... tossing");
82 fprintf(stderr,
"\nvrpn_Tracker_Flock: No valid connection");
89 #define ONE_SEC (1000000l)
92 (
void * userdata,
const vrpn_ANALOGCB b)
94 double tx, ty, tz, rx, ry, rz;
102 printf(
"Joy total = %d,Chan[0] = %lf\n",
103 b.num_channel,b.channel[0]);
105 if (pts->prevtime.tv_sec == -1) {
108 deltaT = (pts->prevtime.tv_sec*
ONE_SEC + pts->prevtime.tv_usec) -
109 (b.msg_time.tv_sec *
ONE_SEC + b.msg_time.tv_usec);
113 memcpy(&(pts->prevtime), &b.msg_time,
sizeof(
struct timeval));
115 for (i=0; i< 7; i++) {
118 if (!(b.channel[i] >=-0.5 && b.channel[i] <= 0.5))
return;
119 for (j=0; j< pts->chanPower[i]; j++)
120 temp[i] *= b.channel[i];
121 if (b.channel[i] > 0) {
122 temp[i] *= pts->chanAccel[i];
124 temp[i] = -fabs(temp[i]) * pts->chanAccel[i];
144 q_euler_to_col_matrix(newM, rz, ry, rx);
145 newM[3][0] = tx; newM[3][1] = ty; newM[3][2] = tz;
155 q_matrix_mult(
final, newM, currentMatrix);
156 q_matrix_copy(currentMatrix,
final);
157 q_row_matrix_to_xyz_quat( & xq, currentMatrix);
161 for (i=0; i< 3; i++) {
164 printf(
"(x, y, z)= %lf %lf %lf\n",
pos[0],
pos[1],
pos[2]);
165 for (i=0; i< 4; i++) {
175 printf(
"Get a new connection, reset virtual_Tracker\n");
181 q_matrix_copy(currentMatrix, initMatrix);
182 prevtime.tv_sec = -1;
183 prevtime.tv_usec = -1;