/** Some VRPN orientation sensors may be rotated differently than what we * expect them to be (for example, orientation is correct except that * the camera is pointing in the wrong direction). This function will * adjust the orientation matrix so that the camera is pointing in the * correct direction. */ static void viewmat_fix_rotation(float orient[16]) { // Fix rotation for a hard-wired orientation sensor. if(viewmat_control_mode == VIEWMAT_CONTROL_ORIENT) { float adjustLeft[16] = { 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 1 }; mat4f_transpose(adjustLeft); // transpose to column-major order float adjustRight[16] = { 0, 0, -1, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1 }; mat4f_transpose(adjustRight); mat4f_mult_mat4f_new(orient, adjustLeft, orient); mat4f_mult_mat4f_new(orient, orient, adjustRight); return; } // Fix rotation for VRPN if(viewmat_vrpn_obj == NULL || strlen(viewmat_vrpn_obj) == 0) return; char *hostname = vrpn_default_host(); if(hostname == NULL) return; /* Some objects in the IVS lab need to be rotated to match the * orientation that we expect. Apply the fix here. */ if(vrpn_is_vicon(hostname)) // MTU vicon tracker { /* Note, orient has not been transposed/inverted yet. Doing * orient*offset will effectively effectively be rotating the * camera---not the world. */ if(strcmp(viewmat_vrpn_obj, "DK2") == 0) { float offsetVicon[16]; mat4f_identity(offsetVicon); mat4f_rotateAxis_new(offsetVicon, 90, 1,0,0); mat4f_mult_mat4f_new(orient, orient, offsetVicon); } if(strcmp(viewmat_vrpn_obj, "DSight") == 0) { float offsetVicon1[16]; mat4f_identity(offsetVicon1); mat4f_rotateAxis_new(offsetVicon1, 90, 1,0,0); float offsetVicon2[16]; mat4f_identity(offsetVicon2); mat4f_rotateAxis_new(offsetVicon2, 180, 0,1,0); // orient = orient * offsetVicon1 * offsetVicon2 mat4f_mult_mat4f_many(orient, orient, offsetVicon1, offsetVicon2, NULL); } } }
/** Uses the VRPN library to get the position and orientation of a * tracked object. * * @param object The name of the object being tracked. * * @param hostname The IP address or hostname of the VRPN server or * tracking system computer. If hostname is set to NULL, the * ~/.vrpn-server file is consulted. * * @param pos An array to be filled in with the position information * for the tracked object. If we are unable to track the object, a * message may be printed and pos will be set to a fixed value. * * @param orient An array to be filled in with the orientation matrix * for the tracked object. The orientation matrix is in row-major * order can be used with OpenGL. If the tracking system is moving an * object around on the screen, this matrix can be used directly. If * the tracking system is moving the OpenGL camera, this matrix may * need to be inverted. If we are unable to track the object, a * message may be printed and orient will be set to the identity * matrix. * * @return 1 if we returned data from the tracker. 0 if there was * problems connecting to the tracker. */ int vrpn_get(const char *object, const char *hostname, float pos[3], float orient[16]) { /* Set to default values */ vec3f_set(pos, 10000,10000,10000); mat4f_identity(orient); #ifdef MISSING_VRPN printf("You are missing VRPN support.\n"); return 0; #else if(object == NULL || strlen(object) == 0) { msg(WARNING, "Empty or NULL object name was passed into this function.\n"); return 0; } if(hostname != NULL && strlen(hostname) == 0) { msg(WARNING, "Hostname is an empty string.\n"); return 0; } /* Construct an object@hostname string. */ std::string hostnamecpp; std::string objectcpp; if(hostname == NULL) { char *hostnameInFile = vrpn_default_host(); if(hostnameInFile) hostnamecpp = hostnameInFile; else { msg(ERROR, "Failed to find hostname of VRPN server.\n"); exit(EXIT_FAILURE); } } else hostnamecpp = hostname; objectcpp = object; std::string fullname = objectcpp + "@" + hostnamecpp; /* Check if we have a tracker object for that string in our map. */ if(nameToTracker.count(fullname)) { /* If we already have a tracker object, ask it to run the main * loop (and therefore call our handle_tracker() function if * there is new data). */ nameToTracker[fullname]->mainloop(); /* If our callback has been called, get the callback object * and get the data out of it. */ if(nameToCallbackData.count(fullname)) { vrpn_TRACKERCB t = nameToCallbackData[fullname]; float pos4[4]; for(int i=0; i<3; i++) pos4[i] = t.pos[i]; pos4[3]=1; double orientd[16]; // Convert quaternion into orientation matrix. q_to_ogl_matrix(orientd, t.quat); for(int i=0; i<16; i++) orient[i] = (float) orientd[i]; /* VICON in the MTU IVS lab is typically calibrated so that: * X = points to the right (while facing screen) * Y = points into the screen * Z = up * (left-handed coordinate system) * * PPT is typically calibrated so that: * X = the points to the wall that has two closets at both corners * Y = up * Z = points to the door * (right-handed coordinate system) * * By default, OpenGL assumes that: * X = points to the right (while facing screen in the IVS lab) * Y = up * Z = points OUT of the screen (i.e., -Z points into the screen in te IVS lab) * (right-handed coordinate system) * * Below, we convert the position and orientation * information into the OpenGL convention. */ if(strlen(hostnamecpp.c_str()) > 14 && strncmp(hostnamecpp.c_str(), "tcp://141.219.", 14) == 0) // MTU vicon tracker { float viconTransform[16] = { 1,0,0,0, // column major order! 0,0,-1,0, 0,1,0,0, 0,0,0,1 }; mat4f_mult_mat4f_new(orient, viconTransform, orient); mat4f_mult_vec4f_new(pos4, viconTransform, pos4); vec3f_copy(pos,pos4); return 1; // we successfully collected some data } else // Non-Vicon tracker { /* Don't transform other tracking systems */ // orient is already filled in vec3f_copy(pos, pos4); return 1; // we successfully collected some data } } } else { /* If this is our first time, create a tracker for the object@hostname string, register the callback handler. */ msg(INFO, "Connecting to VRPN server: %s\n", hostnamecpp.c_str()); // If we are making a TCP connection and the server isn't up, the following function call may hang for a long time vrpn_Connection *connection = vrpn_get_connection_by_name(hostnamecpp.c_str()); /* Wait for a bit to see if we can connect. Sometimes we don't immediately connect! */ for(int i=0; i<1000 && !connection->connected(); i++) { usleep(1000); // 1000 microseconds * 1000 = up to 1 second of waiting. connection->mainloop(); } /* If connection failed, exit. */ if(!connection->connected()) { delete connection; msg(ERROR, "Failed to connect to tracker: %s\n", fullname.c_str()); return 0; } vrpn_Tracker_Remote *tkr = new vrpn_Tracker_Remote(fullname.c_str(), connection); nameToTracker[fullname] = tkr; tkr->register_change_handler((void*) fullname.c_str(), handle_tracker); kuhl_getfps_init(&fps_state); kalman_initialize(&kalman, 0.1, 0.1); } return 0; #endif }