예제 #1
0
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);
	}
예제 #2
0
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;
	}
예제 #4
0
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;
	}
	}
예제 #5
0
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));
		}
	}
예제 #6
0
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));
			}
		}
	}
예제 #7
0
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;
		}
	}
예제 #8
0
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]);
		}
예제 #9
0
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]);
			}
		}
		}
	}
예제 #11
0
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);
		}
	}
예제 #12
0
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);
		}
	}
예제 #13
0
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: */
					;
				}
			}
		}
	}
예제 #14
0
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();
		}
예제 #15
0
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;
				}
			}
		}
	}