void PolhemusFastrak::processBuffer(int station,const char* recordBuffer)
{
Vrui::VRDeviceState::TrackerState ts;
/* Calculate raw position and orientation: */
typedef PositionOrientation::Vector Vector;
typedef Vector::Scalar VScalar;
Vector v;
v[0]=VScalar(readFloat(recordBuffer+8));
v[1]=VScalar(readFloat(recordBuffer+12));
v[2]=VScalar(readFloat(recordBuffer+16));
typedef PositionOrientation::Rotation Rotation;
typedef Rotation::Scalar RScalar;
RScalar angles[3];
angles[0]=Math::rad(RScalar(readFloat(recordBuffer+20)));
angles[1]=Math::rad(RScalar(readFloat(recordBuffer+24)));
angles[2]=Math::rad(RScalar(readFloat(recordBuffer+28)));
Rotation o=Rotation::identity;
o*=Rotation::rotateZ(angles[0]);
o*=Rotation::rotateY(angles[1]);
o*=Rotation::rotateX(angles[2]);
/* Set new position and orientation: */
ts.positionOrientation=Vrui::VRDeviceState::TrackerState::PositionOrientation(v,o);
/* Calculate linear and angular velocities: */
timers[station].elapse();
if(notFirstMeasurements[station])
{
/* Estimate velocities by dividing position/orientation differences by elapsed time since last measurement: */
double time=timers[station].getTime();
ts.linearVelocity=(v-oldPositionOrientations[station].getTranslation())/Vrui::VRDeviceState::TrackerState::LinearVelocity::Scalar(time);
Rotation dO=o*Geometry::invert(oldPositionOrientations[station].getRotation());
ts.angularVelocity=dO.getScaledAxis()/Vrui::VRDeviceState::TrackerState::AngularVelocity::Scalar(time);
}
else
{
/* Force initial velocities to zero: */
ts.linearVelocity=Vrui::VRDeviceState::TrackerState::LinearVelocity::zero;
ts.angularVelocity=Vrui::VRDeviceState::TrackerState::AngularVelocity::zero;
notFirstMeasurements[station]=true;
}
oldPositionOrientations[station]=ts.positionOrientation;
/* Update button state: */
if(station==0&&stylusEnabled)
{
/* Update stylus button state: */
setButtonState(0,recordBuffer[33]=='1');
}
/* Update tracker state: */
setTrackerState(station,ts);
}
void VRPNClient::updateTrackerVelocity(int trackerIndex,const LinearVelocity& linearVelocity,const AngularVelocity& angularVelocity)
{
trackerStates[trackerIndex].linearVelocity=linearVelocity;
trackerStates[trackerIndex].angularVelocity=angularVelocity;
trackerFlags[trackerIndex]|=0x2;
// if(trackerFlags[trackerIndex]==0x3)
{
if(reportEvents)
setTrackerState(trackerIndex,trackerStates[trackerIndex]);
trackerFlags[trackerIndex]=0x0;
}
}
void RazerHydraDevice::start(void)
{
/* Set device manager's device states to current device values: */
Threads::Mutex::Lock deviceValuesLock(deviceValuesMutex);
for(int i=0;i<7*2;++i)
setButtonState(i,deviceButtonStates[i]);
for(int i=0;i<3*2;++i)
setValuatorState(i,deviceValuatorStates[i]);
for(int i=0;i<2;++i)
setTrackerState(i,deviceTrackerStates[i]);
/* Start reporting events to the device manager: */
reportEvents=true;
}
void VRPNClient::start(void)
{
/* Set device manager's states to current states: */
{
Threads::Mutex::Lock stateLock(stateMutex);
for(int i=0;i<getNumTrackers();++i)
setTrackerState(i,trackerStates[i]);
for(int i=0;i<getNumButtons();++i)
setButtonState(i,buttonStates[i]);
for(int i=0;i<getNumValuators();++i)
setValuatorState(i,valuatorStates[i]);
/* Start reporting events to the device manager: */
reportEvents=true;
}
}
void DummyDevice::deviceThreadMethod(void)
{
while(true)
{
/* Wait for the next status update: */
usleep(sleepTime);
/* 'Update' the device manager's state: */
for(int i=0;i<state.getNumButtons();++i)
setButtonState(i,state.getButtonState(i));
for(int i=0;i<state.getNumValuators();++i)
setValuatorState(i,state.getValuatorState(i));
for(int i=0;i<state.getNumTrackers();++i)
setTrackerState(i,state.getTrackerState(i));
}
}
void RemoteDevice::deviceThreadMethod(void)
{
while(true)
{
/* Wait for next message: */
if(pipe.readMessage()==Vrui::VRDevicePipe::PACKET_REPLY) // Just ignore any other messages
{
/* Read current server state: */
state.read(pipe,false);
/* Copy new state into device manager: */
for(int i=0;i<state.getNumValuators();++i)
setValuatorState(i,state.getValuatorState(i));
for(int i=0;i<state.getNumButtons();++i)
setButtonState(i,state.getButtonState(i));
for(int i=0;i<state.getNumTrackers();++i)
setTrackerState(i,state.getTrackerState(i));
}
}
}
void VRPNClient::updateTrackerPosition(int trackerIndex,const PositionOrientation& positionOrientation)
{
/* Check if the current position/orientation is the "fallback position:" */
bool fallbackPosition=true;
for(int i=0;i<3&&fallbackPosition;++i)
fallbackPosition=positionOrientation.getTranslation()[i]==0.0;
for(int i=0;i<3&&fallbackPosition;++i)
fallbackPosition=positionOrientation.getRotation().getQuaternion()[i]==0.0;
if(fallbackPosition)
fallbackPosition=positionOrientation.getRotation().getQuaternion()[3]==1.0;
/* Only update the current position if the new position is not the fallback: */
if(!fallbackPosition)
trackerStates[trackerIndex].positionOrientation=positionOrientation;
/* Notify the device manager that an update has arrived (even if it was ignored): */
trackerFlags[trackerIndex]|=0x1;
// if(trackerFlags[trackerIndex]==0x3)
{
if(reportEvents)
setTrackerState(trackerIndex,trackerStates[trackerIndex]);
trackerFlags[trackerIndex]=0x0;
}
}
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 InterSense::processBuffer(int station,const char* recordBuffer)
{
Vrui::VRDeviceState::TrackerState ts;
/* Calculate raw position and orientation: */
typedef PositionOrientation::Vector Vector;
typedef Vector::Scalar VScalar;
Vector v;
v[0]=VScalar(readFloat(recordBuffer+8));
v[1]=VScalar(readFloat(recordBuffer+12));
v[2]=VScalar(readFloat(recordBuffer+16));
typedef PositionOrientation::Rotation Rotation;
typedef Rotation::Scalar RScalar;
RScalar angles[3];
angles[0]=Math::rad(RScalar(readFloat(recordBuffer+20)));
angles[1]=Math::rad(RScalar(readFloat(recordBuffer+24)));
angles[2]=Math::rad(RScalar(readFloat(recordBuffer+28)));
Rotation o=Rotation::identity;
o*=Rotation::rotateZ(angles[0]);
o*=Rotation::rotateY(angles[1]);
o*=Rotation::rotateX(angles[2]);
/* Set new position and orientation: */
ts.positionOrientation=Vrui::VRDeviceState::TrackerState::PositionOrientation(v,o);
/* Calculate linear and angular velocities: */
timers[station].elapse();
if(notFirstMeasurements[station])
{
/* Estimate velocities by dividing position/orientation differences by elapsed time since last measurement: */
double time=timers[station].getTime();
ts.linearVelocity=(v-oldPositionOrientations[station].getTranslation())/Vrui::VRDeviceState::TrackerState::LinearVelocity::Scalar(time);
Rotation dO=o*Geometry::invert(oldPositionOrientations[station].getRotation());
ts.angularVelocity=dO.getScaledAxis()/Vrui::VRDeviceState::TrackerState::AngularVelocity::Scalar(time);
}
else
{
/* Force initial velocities to zero: */
ts.linearVelocity=Vrui::VRDeviceState::TrackerState::LinearVelocity::zero;
ts.angularVelocity=Vrui::VRDeviceState::TrackerState::AngularVelocity::zero;
notFirstMeasurements[station]=true;
}
oldPositionOrientations[station]=ts.positionOrientation;
/* Update button states: */
for(int i=0;i<stations[station].numButtons;++i)
setButtonState(stations[station].firstButtonIndex+i,recordBuffer[32]&(1<<i));
/* Update valuator states: */
if(stations[station].joystick)
{
float x=(float(*(unsigned char*)(recordBuffer+33))-127.5f)/127.5f;
float y=(float(*(unsigned char*)(recordBuffer+34))-127.5f)/127.5f;
setValuatorState(stations[station].firstValuatorIndex+0,x);
setValuatorState(stations[station].firstValuatorIndex+1,y);
}
/* Update tracker state: */
setTrackerState(station,ts);
}
void RazerHydraDevice::deviceThreadMethod(void)
{
typedef PositionOrientation::Vector Vector;
typedef PositionOrientation::Rotation Rotation;
/* Reset first measurement flag: */
notFirstMeasurement=false;
while(keepRunning)
{
/* Wait for the next update message: */
RazerHydra::SensorState sensorStates[2];
hydra->pollSensors(sensorStates);
/* Update the device tracker state structures: */
{
Threads::Mutex::Lock deviceValuesLock(deviceValuesMutex);
timer.elapse();
for(int sensor=0;sensor<2;++sensor)
{
/* Copy the sensor's button and valuator states: */
for(int i=0;i<7;++i)
deviceButtonStates[sensor*7+i]=sensorStates[sensor].buttonStates[i];
for(int i=0;i<3;++i)
deviceValuatorStates[sensor*3+i]=sensorStates[sensor].valuatorStates[i];
/* Copy the sensor's position and orientation: */
Vector t=Vector(sensorStates[sensor].position);
Rotation r=sensorStates[sensor].orientation;
deviceTrackerStates[sensor].positionOrientation=PositionOrientation(t,r);
/* Calculate linear and angular velocities: */
if(notFirstMeasurement)
{
/* Estimate velocities by dividing position/orientation differences by elapsed time since last measurement: */
double time=timer.getTime();
deviceTrackerStates[sensor].linearVelocity=(t-oldPositionOrientations[sensor].getTranslation())/TrackerState::LinearVelocity::Scalar(time);
Rotation dR=r*Geometry::invert(oldPositionOrientations[sensor].getRotation());
deviceTrackerStates[sensor].angularVelocity=dR.getScaledAxis()/TrackerState::AngularVelocity::Scalar(time);
}
else
{
/* Force initial velocities to zero: */
deviceTrackerStates[sensor].linearVelocity=TrackerState::LinearVelocity::zero;
deviceTrackerStates[sensor].angularVelocity=TrackerState::AngularVelocity::zero;
notFirstMeasurement=true;
}
oldPositionOrientations[sensor]=deviceTrackerStates[sensor].positionOrientation;
}
/* Update device state in device manager: */
if(reportEvents)
{
for(int i=0;i<7*2;++i)
setButtonState(i,deviceButtonStates[i]);
for(int i=0;i<3*2;++i)
setValuatorState(i,deviceValuatorStates[i]);
for(int i=0;i<2;++i)
setTrackerState(i,deviceTrackerStates[i]);
}
}
}
}
void AscensionFlockOfBirds::deviceThreadMethod(void)
{
/* Reset first measurement flags: */
for(int i=0;i<numTrackers;++i)
notFirstMeasurements[i]=false;
/* Process packets until killed: */
while(true)
{
/* Read the next message: */
char buffer[13];
devicePort.readBytes(13,buffer);
/* Check for sync: */
if(!(buffer[0]&0x80))
{
/* Lost sync - discard the read data and wait for the next record start: */
#ifdef VERBOSE
printf("AscensionFlockOfBirds: Re-synchronizing with data stream\n");
fflush(stdout);
#endif
while(!(buffer[0]&0x80))
buffer[0]=devicePort.readByte();
/* Read the rest of the record: */
devicePort.readBytes(12,buffer+1);
}
/* Extract data from the buffer: */
int deviceTrackerIndex=int(buffer[12])-1;
Vrui::VRDeviceState::TrackerState ts;
/* Calculate raw position and orientation: */
typedef PositionOrientation::Vector Vector;
typedef Vector::Scalar VScalar;
VScalar vFactor=VScalar(trackerRange)/VScalar(32767);
Vector v;
v[0]=-VScalar(extractShort(buffer+0))*vFactor; // Hack: FOB seems to deliver left-hand coordinate system!
v[1]=VScalar(extractShort(buffer+2))*vFactor;
v[2]=VScalar(extractShort(buffer+4))*vFactor;
typedef PositionOrientation::Rotation Rotation;
typedef Rotation::Scalar RScalar;
RScalar rFactor=Math::Constants<RScalar>::pi/RScalar(32767);
RScalar angles[3];
angles[2]=-RScalar(extractShort(buffer+6))*rFactor;
angles[1]=-RScalar(extractShort(buffer+8))*rFactor;
angles[0]=RScalar(extractShort(buffer+10))*rFactor;
Rotation o=Rotation::identity;
o*=Rotation::rotateZ(angles[2]);
o*=Rotation::rotateY(angles[1]);
o*=Rotation::rotateX(angles[0]);
/* Set new position and orientation: */
ts.positionOrientation=PositionOrientation(v,o);
/* Calculate linear and angular velocities: */
timers[deviceTrackerIndex].elapse();
if(notFirstMeasurements[deviceTrackerIndex])
{
/* Estimate velocities by dividing position/orientation differences by elapsed time since last measurement: */
double time=timers[deviceTrackerIndex].getTime();
ts.linearVelocity=(v-oldPositionOrientations[deviceTrackerIndex].getTranslation())/Vrui::VRDeviceState::TrackerState::LinearVelocity::Scalar(time);
Rotation dO=o*Geometry::invert(oldPositionOrientations[deviceTrackerIndex].getRotation());
ts.angularVelocity=dO.getScaledAxis()/Vrui::VRDeviceState::TrackerState::AngularVelocity::Scalar(time);
}
else
{
/* Force initial velocities to zero: */
ts.linearVelocity=Vrui::VRDeviceState::TrackerState::LinearVelocity::zero;
ts.angularVelocity=Vrui::VRDeviceState::TrackerState::AngularVelocity::zero;
notFirstMeasurements[deviceTrackerIndex]=true;
}
oldPositionOrientations[deviceTrackerIndex]=ts.positionOrientation;
/* Update tracker state: */
setTrackerState(deviceTrackerIndex,ts);
}
}
void WiimoteTracker::wiimoteEventCallback(Misc::CallbackData* cbData)
{
/* Read the current instantaneous acceleration vector: */
Vector newAcceleration=wiimote->getAcceleration(0);
/* Update the filtered acceleration vector: */
if(firstEvent)
acceleration=newAcceleration;
else
{
Vector da=newAcceleration-lastAcceleration;
Scalar trust=Math::exp(-Geometry::sqr(da)*Scalar(50))*Scalar(0.2);
acceleration+=(newAcceleration-acceleration)*trust;
}
lastAcceleration=newAcceleration;
/* Calculate an intermediate rotation based on the filtered acceleration vector: */
Vector previousY=wiipos.getDirection(1);
Scalar yaw=Math::acos(previousY[1]/Math::sqrt(Math::sqr(previousY[0])+Math::sqr(previousY[1])));
if(previousY[0]>Scalar(0))
yaw=-yaw;
Scalar axz=Math::sqrt(Math::sqr(acceleration[0])+Math::sqr(acceleration[2]));
Scalar roll=Math::acos(acceleration[2]/axz);
if(acceleration[0]>Scalar(0))
roll=-roll;
Scalar pitch=Math::acos(axz/Math::sqrt(Math::sqr(acceleration[1])+Math::sqr(axz)));
if(acceleration[1]<Scalar(0))
pitch=-pitch;
Transform::Rotation wiirot=Transform::Rotation::rotateZ(yaw);
wiirot*=Transform::Rotation::rotateX(pitch);
wiirot*=Transform::Rotation::rotateY(roll);
/* Update the wiimote's orientation based on the acceleration vector only: */
wiipos=Transform(wiipos.getTranslation(),wiirot);
/* Store the IR camera targets: */
int numValidTargets=0;
for(int i=0;i<4;++i)
{
pixelValids[i]=wiimote->getIRTarget(i).valid;
if(pixelValids[i])
{
for(int j=0;j<2;++j)
pixels[i][j]=Scalar(wiimote->getIRTarget(i).pos[j]);
++numValidTargets;
}
}
if(numValidTargets>0)
{
if(numValidTargets==4)
{
/* Project the "up" vector into camera space: */
typedef Geometry::Vector<CameraFitter::Scalar,2> PVector;
PVector vy(acceleration[0],acceleration[2]);
vy.normalize();
PVector vx=-Geometry::normal(vy);
vx.normalize();
/* Find the leftmost, rightmost, and topmost points: */
Scalar minX,maxX,minY,maxY;
int minXIndex,maxXIndex,minYIndex,maxYIndex;
minX=minY=Math::Constants<Scalar>::max;
maxX=maxY=Math::Constants<Scalar>::min;
minXIndex=maxXIndex=minYIndex=maxYIndex=-1;
for(int i=0;i<4;++i)
{
Scalar x=pixels[i]*vx;
Scalar y=pixels[i]*vy;
if(minX>x)
{
minX=x;
minXIndex=i;
}
if(maxX<x)
{
maxX=x;
maxXIndex=i;
}
if(minY>y)
{
minY=y;
minYIndex=i;
}
if(maxY<y)
{
maxY=y;
maxYIndex=i;
}
}
/* Create the pixel-target map: */
pixelMap[minXIndex]=0;
pixelMap[maxYIndex]=1;
pixelMap[maxXIndex]=2;
for(int i=0;i<4;++i)
if(i!=minXIndex&&i!=maxYIndex&&i!=maxXIndex)
pixelMap[i]=3;
}
else
{
/* Project the target points into camera space using the previous camera position/orientation and match closest pairs: */
wiiCamera.setTransform(wiipos);
for(int pixelIndex=0;pixelIndex<4;++pixelIndex)
if(pixelValids[pixelIndex])
{
Scalar minDist2=Geometry::sqrDist(pixels[pixelIndex],wiiCamera.project(0));
int minIndex=0;
for(int i=1;i<4;++i)
{
Scalar dist2=Geometry::sqrDist(pixels[pixelIndex],wiiCamera.project(i));
if(minDist2>dist2)
{
minDist2=dist2;
minIndex=i;
}
}
pixelMap[pixelIndex]=minIndex;
}
}
/* Re-project the new pixel positions: */
wiiCamera.setTransform(homeTransform);
for(int i=0;i<4;++i)
wiiCamera.invalidatePixel(i);
for(int i=0;i<4;++i)
if(pixelValids[i])
wiiCamera.setPixel(pixelMap[i],pixels[i]);
wiiCamera.setTransform(homeTransform);
wiiCameraMinimizer.minimize(wiiCamera);
if(firstEvent)
wiipos=wiiCamera.getTransform();
else
{
/* Filter the reconstructed camera transformation: */
Transform deltaWP=Geometry::invert(wiipos);
deltaWP.leftMultiply(wiiCamera.getTransform());
Vector t=deltaWP.getTranslation();
t*=Scalar(0.05);
Vector r=deltaWP.getRotation().getScaledAxis();
r*=Scalar(0.05);
deltaWP=Transform(t,Transform::Rotation::rotateScaledAxis(r));
wiipos.leftMultiply(deltaWP);
}
}
wiipos.renormalize();
firstEvent=false;
if(wiimote->getButtonState(Wiimote::BUTTON_HOME))
wiipos=homeTransform;
if(reportEvents)
{
/* Update the VR device state: */
for(int i=0;i<13;++i)
setButtonState(i,wiimote->getButtonState(i));
for(int i=0;i<2;++i)
setValuatorState(i,wiimote->getJoystickValue(i));
Vrui::VRDeviceState::TrackerState ts;
ts.positionOrientation=PositionOrientation(wiipos);
ts.linearVelocity=Vrui::VRDeviceState::TrackerState::LinearVelocity::zero;
ts.angularVelocity=Vrui::VRDeviceState::TrackerState::AngularVelocity::zero;
setTrackerState(0,ts);
}
}
void ViconTarsus::deviceThreadMethod(void)
{
while(true)
{
/* Wait for the next packet from the server: */
int packetKind=pipe.read<int>();
if(pipe.read<int>()==1) // Ignore request packets
{
switch(packetKind)
{
case 0:
case 4:
/* Shut down the thread method: */
return;
case 2:
{
/* Read the data packet: */
int numPacketChannels=pipe.read<int>();
if(numPacketChannels>numChannels)
{
/* Read the relevant channels: */
pipe.read<double>(channelPacketBuffer,numChannels);
/* Ignore all spurious channels: */
for(int i=numChannels;i<numPacketChannels;++i)
pipe.read<double>();
numPacketChannels=numChannels;
}
else
{
/* Read all channels: */
pipe.read<double>(channelPacketBuffer,numPacketChannels);
}
/* Process the data packet: */
for(int trackerIndex=0;trackerIndex<getNumTrackers();++trackerIndex)
{
/* Get the tracker's position: */
Vector translation;
bool valid=true;
for(int i=0;i<3;++i)
{
if(trackerChannelIndices[trackerIndex*6+i]<numPacketChannels)
translation[i]=VScalar(channelPacketBuffer[trackerChannelIndices[trackerIndex*6+i]]);
else
valid=false;
}
if(valid)
{
if(trackerSixDofs[trackerIndex])
{
/* Get the tracker's orientation: */
PositionOrientation::Rotation::Vector rotation;
bool sixDof=true;
for(int i=0;i<3;++i)
{
if(trackerChannelIndices[trackerIndex*6+3+i]<numPacketChannels)
rotation[i]=RScalar(channelPacketBuffer[trackerChannelIndices[trackerIndex*6+3+i]]);
else
sixDof=false;
}
if(sixDof)
{
/* Set the 6-DOF tracker state: */
trackerStates[trackerIndex].positionOrientation=PositionOrientation(translation,Rotation::rotateScaledAxis(rotation));
}
else
{
/* Set the 6-DOF tracker state with the previous orientation: */
trackerStates[trackerIndex].positionOrientation=PositionOrientation(translation,trackerStates[trackerIndex].positionOrientation.getRotation());
}
}
else
{
/* Set the 3-DOF tracker state: */
trackerStates[trackerIndex].positionOrientation=PositionOrientation(translation,Rotation::identity);
}
}
}
/* Update all tracker states (including those that were not updated): */
for(int i=0;i<getNumTrackers();++i)
setTrackerState(i,trackerStates[i]);
break;
}
default:
/* Ignore the packet: */
;
}
}
}
}
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();
}
void SpaceBall::deviceThreadMethod(void)
{
/* Create free-running timer to estimate tracker velocities: */
Misc::Timer timer;
bool notFirstMeasurement=false;
/* Receive lines from the serial port until interrupted: */
while(true)
{
/* Read characters until an end-of-line is encountered: */
unsigned char packet[256];
readPacket(256,packet);
/* Determine the packet type: */
switch(packet[0])
{
case 'D':
{
Vrui::VRDeviceState::TrackerState ts;
/* Parse a data packet: */
short int rawData[6];
rawData[0]=(short int)(((unsigned int)packet[ 3]<<8)|(unsigned int)packet[ 4]);
rawData[1]=(short int)(((unsigned int)packet[ 5]<<8)|(unsigned int)packet[ 6]);
rawData[2]=(short int)(((unsigned int)packet[ 7]<<8)|(unsigned int)packet[ 8]);
rawData[3]=(short int)(((unsigned int)packet[ 9]<<8)|(unsigned int)packet[10]);
rawData[4]=(short int)(((unsigned int)packet[11]<<8)|(unsigned int)packet[12]);
rawData[5]=(short int)(((unsigned int)packet[13]<<8)|(unsigned int)packet[14]);
/* Calibrate linear data: */
PositionOrientation::Vector translation;
for(int i=0;i<3;++i)
translation[i]=double(rawData[i])*linearGain;
translation[2]=-translation[2]; // Z-axis values are negated (?!?)
/* Calibrate angular data: */
PositionOrientation::Vector rotationAxis;
for(int i=0;i<3;++i)
rotationAxis[i]=double(rawData[i+3])*angularGain;
rotationAxis[2]=-rotationAxis[2]; // Z-axis values are negated (?!?)
/* Construct incremental transformation: */
PositionOrientation t=PositionOrientation::translate(translation);
PositionOrientation::Scalar rotationAngle=Geometry::mag(rotationAxis);
PositionOrientation::Rotation rotation=PositionOrientation::Rotation::rotateAxis(rotationAxis,rotationAngle);
t*=PositionOrientation::rotate(rotation);
/* Accumulate current device position/orientation: */
currentPositionOrientation*=t;
// currentPositionOrientation.leftMultiply(t);
ts.positionOrientation=currentPositionOrientation;
/* Calculate linear and angular velocities: */
timer.elapse();
if(notFirstMeasurement)
{
/* Estimate velocities by dividing position/orientation differences by elapsed time since last measurement: */
double time=timer.getTime();
ts.linearVelocity=translation/Vrui::VRDeviceState::TrackerState::LinearVelocity::Scalar(time);
ts.angularVelocity=rotationAxis/Vrui::VRDeviceState::TrackerState::AngularVelocity::Scalar(time);
}
else
{
/* Force initial velocities to zero: */
ts.linearVelocity=Vrui::VRDeviceState::TrackerState::LinearVelocity::zero;
ts.angularVelocity=Vrui::VRDeviceState::TrackerState::AngularVelocity::zero;
notFirstMeasurement=true;
}
/* Update tracker state: */
setTrackerState(0,ts);
break;
}
case '.':
{
/* Parse a button event packet: */
int buttonMask=0x0;
buttonMask|=int(packet[2]&0x3f);
buttonMask|=int(packet[2]&0x80)>>1;
buttonMask|=int(packet[1]&0x1f)<<7;
/* Update the current button states: */
for(int i=0;i<12;++i)
setButtonState(i,buttonMask&(1<<i));
break;
}
}
}
}
