void vrpn_Sound_Server_A3D::setListenerPose(vrpn_PoseDef pose) { qogl_matrix_type eye_f_world_mat; qogl_matrix_type eye_axis_vectors_in_world; q_to_ogl_matrix (eye_f_world_mat, pose.orientation); qogl_matrix_type eye_axis_vectors=// we put the up and forward // as the first two columns in the matrix. //NOTE: these columns in the matrix //appear as rows here since the number starts in the upper left corner //as goes down! { 0,0,-1,1, //the forward normal vector in eye space 0,1,0,1, //the up normal vector in eye space 0,0,0,0, //unused 0,0,0,0 } ; //unused qogl_matrix_mult(eye_axis_vectors_in_world,eye_axis_vectors,eye_f_world_mat); // takes front vector then up vector lasterror = a3dlis->SetOrientation6f((float)eye_axis_vectors_in_world[0], (float)eye_axis_vectors_in_world[1], (float)eye_axis_vectors_in_world[2], (float)eye_axis_vectors_in_world[4], (float)eye_axis_vectors_in_world[5], (float)eye_axis_vectors_in_world[6]); lasterror = a3dlis->SetPosition3f((float)pose.position[0],(float)pose.position[1],(float)pose.position[2]); // dont send back a message here because there are a bunch of them }
void q_xyz_quat_to_ogl_matrix( qogl_matrix_type matrix, q_xyz_quat_type *xyzQuatPtr ) { q_to_ogl_matrix( matrix, xyzQuatPtr->quat ); matrix[12+Q_X] = xyzQuatPtr->xyz[Q_X]; matrix[12+Q_Y] = xyzQuatPtr->xyz[Q_Y]; matrix[12+Q_Z] = xyzQuatPtr->xyz[Q_Z]; return; }
void vrpn_Sound_Server_A3D::changeSoundStatus(vrpn_SoundID id, vrpn_SoundDef soundDef) { vrpn_int32 myid = soundMap[id]; char tempbuf[1024]; if (myid==-1) { sprintf(tempbuf,"Error: setSoundStatus(Invalid id)"); printf("%s\n", tempbuf); send_message((const char *) tempbuf,vrpn_TEXT_ERROR,0); return; } lasterror = a3dsamples[id]->SetPosition3f((float)soundDef.pose.position[0],(float)soundDef.pose.position[1],(float)soundDef.pose.position[2]); qogl_matrix_type eye_f_world_mat; qogl_matrix_type eye_axis_vectors_in_world; q_to_ogl_matrix (eye_f_world_mat, soundDef.pose.orientation); qogl_matrix_type eye_axis_vectors=// we put the up and forward // as the first two columns in the matrix. //NOTE: these columns in the matrix //appear as rows here since the number starts in the upper left corner //as goes down! { 0,0,-1,1, //the forward normal vector in eye space 0,1,0,1, //the up normal vector in eye space 0,0,0,0, //unused 0,0,0,0 } ; //unused qogl_matrix_mult(eye_axis_vectors_in_world,eye_axis_vectors,eye_f_world_mat); // takes front vector then up vector lasterror = a3dsamples[id]->SetOrientation6f((float)eye_axis_vectors_in_world[0], (float)eye_axis_vectors_in_world[1], (float)eye_axis_vectors_in_world[2], (float)eye_axis_vectors_in_world[4], (float)eye_axis_vectors_in_world[5], (float)eye_axis_vectors_in_world[6]); q_vec_type velocity_vec; q_to_vec(velocity_vec, soundDef.velocity); lasterror = a3dsamples[myid]->SetVelocity3fv((A3DVAL*) velocity_vec); // we only use front_min and front_back lasterror = a3dsamples[myid]->SetMinMaxDistance((float)soundDef.min_front_dist, (float)soundDef.max_front_dist, A3D_AUDIBLE); lasterror = a3dsamples[myid]->SetCone((float)soundDef.cone_inner_angle, (float)soundDef.cone_outer_angle, (float)soundDef.cone_gain); lasterror = a3dsamples[myid]->SetDopplerScale((float)soundDef.dopler_scale); lasterror = a3dsamples[myid]->SetEq((float)soundDef.equalization_val); lasterror = a3dsamples[myid]->SetPitch((float)soundDef.pitch); lasterror = a3dsamples[myid]->SetGain((float)soundDef.volume); sprintf(tempbuf,"Setting sound definition "); printf("%s\n", tempbuf); send_message((const char *) tempbuf,vrpn_TEXT_NORMAL,0); return; }
// Callback function for VRPN, void* pointer points to a VRPNTrackerDevice void VRPN_CALLBACK trackerHandler(void *thisPtr, const vrpn_TRACKERCB info) { double rotraw[16]; q_to_ogl_matrix(rotraw, info.quat); VRMatrix4 vrpnEvent(rotraw); // VRMatrix4 vrpnEvent; vrpnEvent[3][0] = info.pos[0]; vrpnEvent[3][1] = info.pos[1]; vrpnEvent[3][2] = info.pos[2]; VRVRPNTrackerDevice* device = ((VRVRPNTrackerDevice*)thisPtr); device->processEvent(vrpnEvent, info.sensor); }
void draw_tracker(q_xyz_quat_type* pq) { // Convert from Tracker coordinates to our scene coordinates. float x = -10.0*(pq->xyz[Q_X]); float y = 10.0*(pq->xyz[Q_Y]); float z = -10.0*(pq->xyz[Q_Z]); double q[4] = { -pq->quat[Q_X], pq->quat[Q_Y], -pq->quat[Q_Z], pq->quat[Q_W] }; // Convert the orientation from quaternion to OpenGL matrix. double rotation[16]; q_to_ogl_matrix(rotation,q); // First draw line to object. glColor3f(1.0, 1.0, 1.0); glBegin(GL_LINES); glVertex3f(0.0, 0.0, 0.0 ); glVertex3f(x, y, z); glEnd(); // Draw the tracker itself as two spheres. glEnable(GL_LIGHTING); glColor3f(1.0, 0.0, 0.2); glPushMatrix(); glTranslatef(x, y, z); glMultMatrixd(rotation); glTranslatef(-1.5, 0.0, 0.0); gluSphere(qobj1, 0.2, 32, 32); glTranslatef(3.0,0.0,0.0); gluSphere(qobj2, 0.2, 32, 32); glPopMatrix(); }
static void VRPN_CALLBACK handle_vrpn_tracker(void *userdata, const vrpn_TRACKERCB t) { VRPNTrackerUserData *data = (VRPNTrackerUserData *)userdata; data->pos[0] = (float) t.pos[0]; data->pos[1] = (float) t.pos[1]; data->pos[2] = (float) t.pos[2]; double q[4]; /* we need to swap X and Z to make it work right */ q[0] = -t.quat[Q_Z]; q[1] = t.quat[Q_Y]; q[2] = t.quat[Q_X]; q[3] = t.quat[Q_W]; double rotation[16]; Matrix4 *orient = data->orient; int i; q_to_ogl_matrix(rotation, q); for(i=0;i<4;i++) orient->mat[4*0+i]=(float)rotation[i]; for(i=0;i<4;i++) orient->mat[4*1+i]=(float)rotation[i+4]; for(i=0;i<4;i++) orient->mat[4*2+i]=(float)rotation[i+8]; for(i=0;i<4;i++) orient->mat[4*3+i]=(float)rotation[i+12]; }
void vrpn_Sound_Server_A3D::setSoundPose(vrpn_SoundID id, vrpn_PoseDef pose) { vrpn_int32 myid = soundMap[id]; char tempbuf[1024]; if (myid==-1) { sprintf(tempbuf,"Error: setSoundPose(Invalid id)"); printf("%s\n", tempbuf); send_message((const char *) tempbuf,vrpn_TEXT_ERROR,0); return; } lasterror = a3dsamples[id]->SetPosition3f((float)pose.position[0],(float)pose.position[1],(float)pose.position[2]); qogl_matrix_type eye_f_world_mat; qogl_matrix_type eye_axis_vectors_in_world; q_to_ogl_matrix (eye_f_world_mat, pose.orientation); qogl_matrix_type eye_axis_vectors=// we put the up and forward // as the first two columns in the matrix. //NOTE: these columns in the matrix //appear as rows here since the number starts in the upper left corner //as goes down! { 0,0,-1,1, //the forward normal vector in eye space 0,1,0,1, //the up normal vector in eye space 0,0,0,0, //unused 0,0,0,0 } ; //unused qogl_matrix_mult(eye_axis_vectors_in_world,eye_axis_vectors,eye_f_world_mat); // takes front vector then up vector lasterror = a3dsamples[id]->SetOrientation6f((float)eye_axis_vectors_in_world[0], (float)eye_axis_vectors_in_world[1], (float)eye_axis_vectors_in_world[2], (float)eye_axis_vectors_in_world[4], (float)eye_axis_vectors_in_world[5], (float)eye_axis_vectors_in_world[6]); sprintf(tempbuf,"Setting sound pose "); printf("%s\n", tempbuf); send_message((const char *) tempbuf,vrpn_TEXT_NORMAL,0); }
/** 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 }
void vrpn_Sound_Server_A3D::loadSoundLocal(char* filename, vrpn_SoundID id, vrpn_SoundDef soundDef) { char tempbuf[1024]; try { numSounds++; if (numSounds == maxSounds) { maxSounds = 2 * maxSounds; IA3dSource2 ** temp = new (IA3dSource2*); for (int i(0); i < maxSounds; i++) { if (i <= numSounds) temp[i] = a3dsamples[i]; else temp[i] = NULL; } delete [] a3dsamples; a3dsamples = temp; } filename = strlwr(filename); // string to lower case lasterror = a3droot->NewSource( A3DSOURCE_TYPEDEFAULT | A3DSOURCE_INITIAL_RENDERMODE_A3D , &a3dsamples[numSounds]); if (FAILED(lasterror)) { throw "Error making new source"; } if (strstr(filename, "mp3")==NULL) // assume WAVE format lasterror = a3dsamples[numSounds]->LoadFile(filename, A3DSOURCE_FORMAT_WAVE); else // otherwise we assume mp3 lasterror = a3dsamples[numSounds]->LoadFile(filename, A3DSOURCE_FORMAT_MP3); if (FAILED(lasterror)) { throw "Error loading file"; } // initialize sound params lasterror=a3dsamples[numSounds]->SetCone(0,0,1); // these values are defaults: // default ModelScale of 6dB for each doubling of distance // default DopplerScale of 1.0 // default Equalization of 1.0 // default pitch is 1.0 (unaltered pitch) // default priority of .5 on a scale from 0 to 1 // default ReflectionDelayScale is 1.0 (normal delay in reflections [.3 seconds]) // default RefelectionGainScale is 1.0 // default RenderMode uses occlusion, reflection, a3d (??) // default TransformMode is relative to origin lasterror=a3dsamples[numSounds]->SetMinMaxDistance((float) soundDef.min_front_dist, (float)soundDef.max_front_dist, A3D_MUTE); lasterror=a3dsamples[numSounds]->SetGain(soundDef.volume); // default position is at the origin lasterror=a3dsamples[numSounds]->SetPosition3f((float)soundDef.pose.position[0],(float)soundDef.pose.position[1],(float)soundDef.pose.position[2]); qogl_matrix_type eye_f_world_mat; qogl_matrix_type eye_axis_vectors_in_world; q_to_ogl_matrix (eye_f_world_mat, soundDef.pose.orientation); qogl_matrix_type eye_axis_vectors=// we put the up and forward // as the first two columns in the matrix. //NOTE: these columns in the matrix //appear as rows here since the number starts in the upper left corner //as goes down! { 0,0,-1,1, //the forward normal vector in eye space 0,1,0,1, //the up normal vector in eye space 0,0,0,0, //unused 0,0,0,0 } ; //unused qogl_matrix_mult(eye_axis_vectors_in_world,eye_axis_vectors,eye_f_world_mat); // takes front vector then up vector lasterror = a3dlis->SetOrientation6f((float)eye_axis_vectors_in_world[0], (float)eye_axis_vectors_in_world[1], (float)eye_axis_vectors_in_world[2], (float)eye_axis_vectors_in_world[4], (float)eye_axis_vectors_in_world[5], (float)eye_axis_vectors_in_world[6]); soundMap[id] = numSounds; } catch(char* szError) { // Display the message. sprintf(tempbuf,"Error: loadSoundLocal (%s) [file: %s]",szError,filename); printf("%s\n", tempbuf); send_message((const char *) tempbuf,vrpn_TEXT_ERROR,0); return; } sprintf(tempbuf,"Loading sound #%d: %s \n",numSounds,filename); printf("%s\n", tempbuf); send_message((const char *) tempbuf,vrpn_TEXT_NORMAL,0); return; }