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;
}
