void ArtDTrack::processAsciiData(void)
{
while(true)
{
/* Wait for the next data message from the DTrack daemon: */
char messageBuffer[4096];
size_t messageSize=dataSocket.receiveMessage(messageBuffer,sizeof(messageBuffer)-1);
/* Newline-terminate the message: */
messageBuffer[messageSize]='\n';
/* Parse the received message: */
char* mPtr=messageBuffer;
char* mEnd=messageBuffer+messageSize;
while(mPtr<mEnd)
{
/* Find the end of the current line: */
char* lineEnd;
for(lineEnd=mPtr;*lineEnd!='\n';++lineEnd)
;
/* Skip whitespace: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
/* Get the line identifier: */
char* idStart=mPtr;
while(mPtr<lineEnd&&!isspace(*mPtr))
++mPtr;
/* Determine the type of the line: */
int lineType=-1;
if(mPtr-idStart==2&&strncasecmp(idStart,"6d",2)==0)
lineType=0;
else if(mPtr-idStart==3&&strncasecmp(idStart,"6df",3)==0)
lineType=1;
else if(mPtr-idStart==4&&strncasecmp(idStart,"6df2",4)==0)
lineType=2;
else if(mPtr-idStart==4&&strncasecmp(idStart,"6dmt",4)==0)
lineType=3;
else if(mPtr-idStart==2&&strncasecmp(idStart,"3d",2)==0)
lineType=4;
/* Process the line: */
if(lineType>=0)
{
if(lineType==2)
{
/* Skip the number of defined flysticks: */
readInt(mPtr);
}
/* Read the number of reported bodies: */
int numBodies=readInt(mPtr);
/* Parse each body: */
for(int body=0;body<numBodies;++body)
{
/* Find the first opening bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!='[')
break; // Ignore the rest of the line
++mPtr;
/* Read the body's ID: */
int id=readInt(mPtr);
Device* device=deviceIdToIndex[id]>=0?&devices[deviceIdToIndex[id]]:0;
/* Skip the quality value: */
float(readFloat(mPtr));
if(lineType==1||lineType==3)
{
/* Read the button bit flag: */
unsigned int bits=readUint(mPtr);
if(device!=0)
{
/* Set the button states: */
for(int i=0;i<32&&i<device->numButtons;++i)
{
setButtonState(device->firstButtonIndex+i,(bits&0x1)!=0x0);
bits>>=1;
}
}
}
int numButtons=0;
int numValuators=0;
if(lineType==2)
{
/* Read the number of buttons and valuators: */
numButtons=readInt(mPtr);
numValuators=readInt(mPtr);
}
/* Find the first closing bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!=']')
break; // Ignore the rest of the line
++mPtr;
/* Find the second opening bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!='[')
break; // Ignore the rest of the line
++mPtr;
/* Read the body's position: */
Vector pos;
for(int i=0;i<3;++i)
pos[i]=VScalar(readFloat(mPtr));
Rotation orient=Rotation::identity;
if(lineType==0||lineType==1)
{
/* Read the body's orientation angles: */
VScalar angles[3];
for(int i=0;i<3;++i)
angles[i]=VScalar(readFloat(mPtr));
/* Calculate the body's orientation quaternion: */
orient*=Rotation::rotateX(Math::rad(angles[0]));
orient*=Rotation::rotateY(Math::rad(angles[1]));
orient*=Rotation::rotateZ(Math::rad(angles[2]));
}
/* Find the second closing bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!=']')
break; // Ignore the rest of the line
++mPtr;
if(lineType!=4)
{
/* Find the third opening bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!='[')
break; // Ignore the rest of the line
++mPtr;
}
if(lineType==2||lineType==3)
{
/* Read the body's orientation matrix (yuck!): */
Geometry::Matrix<VScalar,3,3> matrix;
for(int j=0;j<3;++j)
for(int i=0;i<3;++i)
matrix(i,j)=VScalar(readFloat(mPtr));
/* Calculate the body's orientation quaternion: */
orient=Rotation::fromMatrix(matrix);
/* Find the third closing bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!=']')
break; // Ignore the rest of the line
++mPtr;
}
else if(lineType!=4)
{
/* Ignore the body's orientation matrix: */
while(mPtr<lineEnd&&*mPtr!=']')
++mPtr;
if(*mPtr!=']') // Ignore the rest of the line
break;
}
if(lineType==2)
{
/* Find the fourth opening bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!='[')
break; // Ignore the rest of the line
++mPtr;
/* Read the flystick's button bits: */
for(int bitIndex=0;bitIndex<numButtons;bitIndex+=32)
{
/* Read the next button bit mask: */
unsigned int bits=readUint(mPtr);
if(device!=0)
{
/* Set the device's button values: */
for(int i=0;i<32;++i)
{
/* Set the button state if the button is valid: */
if(bitIndex+i<device->numButtons)
setButtonState(device->firstButtonIndex+bitIndex+i,(bits&0x1)!=0x0);
bits>>=1;
}
}
}
/* Read the flystick's valuator values: */
for(int i=0;i<numValuators;++i)
{
/* Read the next valuator value: */
float value=float(readFloat(mPtr));
/* Set the valuator value if the valuator is valid: */
if(device!=0&&i<device->numValuators)
setValuatorState(device->firstValuatorIndex+i,value);
}
/* Find the fourth closing bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!=']')
break; // Ignore the rest of the line
++mPtr;
}
/* Check if this body has been configured as a device: */
if(device!=0)
{
/* Set the device's tracker state: */
trackerStates[deviceIdToIndex[id]].positionOrientation=PositionOrientation(pos,orient);
}
}
}
/* Go to the start of the next line: */
mPtr=lineEnd+1;
}
/* Update all tracker states (including those that were not updated): */
for(int i=0;i<getNumTrackers();++i)
setTrackerState(i,trackerStates[i]);
}
void ArtDTrack::processAsciiData(void)
{
Vrui::VRDeviceState::TrackerState ts;
ts.linearVelocity=Vrui::VRDeviceState::TrackerState::LinearVelocity::zero;
ts.angularVelocity=Vrui::VRDeviceState::TrackerState::AngularVelocity::zero;
while(true)
{
/* Wait for the next data message from the DTrack daemon: */
char messageBuffer[4096];
size_t messageSize=dataSocket.receiveMessage(messageBuffer,sizeof(messageBuffer)-1);
/* Newline-terminate the message as a sentinel: */
messageBuffer[messageSize]='\n';
/* Parse the received message: */
const char* mPtr=messageBuffer;
const char* mEnd=messageBuffer+(messageSize+1);
while(mPtr!=mEnd)
{
/* Skip whitespace, but not the line terminator: */
while(*mPtr!='\n'&&isspace(*mPtr))
++mPtr;
/* Get the line's device report format: */
DeviceReportFormat drf=parseDeviceReportFormat(mPtr,0,&mPtr);
/* Process the line: */
if(drf!=DRF_NUMFORMATS)
{
if(drf==DRF_6DF2)
{
/* Skip the number of defined flysticks: */
readInt(mPtr);
}
/* Read the number of bodies in this report: */
int numBodies=readInt(mPtr);
/* Parse all body reports: */
for(int body=0;body<numBodies;++body)
{
/* Check for opening bracket: */
if(!expectChar('[',mPtr))
break;
/* Read the body's ID and find the corresponding device structure: */
int id=readInt(mPtr);
int deviceIndex=deviceIdToIndex[drf][id];
Device* device=deviceIndex>=0?&devices[deviceIndex]:0;
/* Read the quality value: */
float quality=float(readFloat(mPtr));
/* Read button/valuator or finger data depending on report format: */
int numButtons=0;
int numValuators=0;
int numFingers=0;
if(drf==DRF_6DF)
{
/* Read the button bit mask: */
unsigned int buttonBits=readUint(mPtr);
if(device!=0)
{
/* Set the device's button states: */
for(int i=0;i<32&&i<device->numButtons;++i,buttonBits>>=1)
setButtonState(device->firstButtonIndex+i,(buttonBits&0x1)!=0x0);
}
}
if(drf==DRF_6DF2||drf==DRF_6DMT)
{
/* Read the number of buttons: */
numButtons=readInt(mPtr);
if(drf==DRF_6DF2)
{
/* Read the number of valuators: */
numValuators=readInt(mPtr);
}
}
if(drf==DRF_GL)
{
/* Skip the glove's handedness: */
readInt(mPtr);
/* Read the number of fingers: */
numFingers=readInt(mPtr);
}
/* Check for closing bracket followed by opening bracket: */
if(!expectChar(']',mPtr)||!expectChar('[',mPtr))
break;
Vector pos;
Rotation orient=Rotation::identity;
/* Read the body's 3D position: */
for(int i=0;i<3;++i)
pos[i]=VScalar(readFloat(mPtr));
if(drf!=DRF_3D)
{
/* Read the body's 3D orientation: */
if(drf==DRF_6D||drf==DRF_6DF)
{
/* Read the body's orientation angles: */
VScalar angles[3];
for(int i=0;i<3;++i)
angles[i]=VScalar(readFloat(mPtr));
/* Convert the orientation angles to a 3D rotation: */
orient*=Rotation::rotateX(Math::rad(angles[0]));
orient*=Rotation::rotateY(Math::rad(angles[1]));
orient*=Rotation::rotateZ(Math::rad(angles[2]));
}
/* Check for closing bracket followed by opening bracket: */
if(!expectChar(']',mPtr)||!expectChar('[',mPtr))
break;
if(drf==DRF_6DF2||drf==DRF_6DMT||drf==DRF_GL)
{
/* Read the body's orientation matrix (yuck!): */
Geometry::Matrix<VScalar,3,3> matrix;
for(int j=0;j<3;++j)
for(int i=0;i<3;++i)
matrix(i,j)=VScalar(readFloat(mPtr));
if(quality>0.0f)
{
/* Calculate the body's orientation quaternion (YUCK!): */
orient=Rotation::fromMatrix(matrix);
}
}
else
{
/* Skip the body's orientation matrix: */
for(int i=0;i<9;++i)
readFloat(mPtr);
}
}
/* Check for closing bracket: */
if(!expectChar(']',mPtr))
break;
if(drf==DRF_6DF2)
{
/* Check for opening bracket: */
if(!expectChar('[',mPtr))
break;
/* Read button states: */
for(int bitIndex=0;bitIndex<numButtons;bitIndex+=32)
{
/* Read the next button bit mask: */
unsigned int buttonBits=readUint(mPtr);
if(device!=0)
{
/* Set the device's button states: */
for(int i=0;i<32&&bitIndex+i<device->numButtons;++i,buttonBits>>=1)
setButtonState(device->firstButtonIndex+bitIndex+i,(buttonBits&0x1)!=0x0);
}
}
/* Read valuator states: */
for(int i=0;i<numValuators;++i)
{
/* Read the next valuator value: */
float value=float(readFloat(mPtr));
/* Set the valuator value if the valuator is valid: */
if(device!=0&&i<device->numValuators)
setValuatorState(device->firstValuatorIndex+i,value);
}
/* Check for closing bracket: */
if(!expectChar(']',mPtr))
break;
}
if(drf==DRF_GL)
{
/* Skip all finger data for now: */
bool error=false;
for(int finger=0;finger<numFingers;++finger)
{
/* Check for opening bracket: */
if(!expectChar('[',mPtr))
{
error=true;
break;
}
/* Skip finger position: */
for(int i=0;i<3;++i)
readFloat(mPtr);
/* Check for closing followed by opening bracket: */
if(!expectChar(']',mPtr)||!expectChar('[',mPtr))
{
error=true;
break;
}
/* Skip finger orientation: */
for(int i=0;i<9;++i)
readFloat(mPtr);
/* Check for closing followed by opening bracket: */
if(!expectChar(']',mPtr)||!expectChar('[',mPtr))
{
error=true;
break;
}
/* Skip finger bending parameters: */
for(int i=0;i<6;++i)
readFloat(mPtr);
/* Check for closing bracket: */
if(!expectChar(']',mPtr))
{
error=true;
break;
}
}
/* Stop parsing the packet on syntax error: */
if(error)
break;
}
/* Check if this body has a valid position/orientation and has been configured as a device: */
if(quality>0.0f&&device!=0)
{
/* Set the device's tracker state: */
ts.positionOrientation=PositionOrientation(pos,orient);
setTrackerState(deviceIndex,ts);
}
}
}
/* Skip the rest of the line: */
while(*mPtr!='\n')
++mPtr;
/* Go to the next line: */
++mPtr;
}
/* Tell the VR device manager that the current state has updated completely: */
updateState();
}
