private void updateTracker(float deltaT)
{
// Lets reduce the amount of required if clauses by setting the following:
if(!externalDriftCorrection)
compass = CompassSource.None;
if(useOculusRiftRotation)
headRotationInput = HeadRotationSource.None;
else if( headRotationInput == HeadRotationSource.Kinect
|| headRotationInput == HeadRotationSource.PSMove
|| headRotationInput == HeadRotationSource.RazerHydra
|| headRotationInput == HeadRotationSource.None )
compass = CompassSource.None; // The above rotation sources do not need yaw drift correction
if( headPositionInput != HeadPositionSource.RazerHydra
&& headRotationInput != HeadRotationSource.RazerHydra
&& compass != CompassSource.RazerHydra )
{
isRazerBaseMobile = false; // If Razer Hydra is not used as a source then this can be false
}
// Reset view if necessary
bool checkRazer = false;
bool checkPSMove = false;
// Reset view: Is PS Move used for tracking?
if (inputManager)
{
if(headPositionInput == HeadPositionSource.PSMove)
{
posePSMove = inputManager.GetMoveWand(positionPSMoveID);
checkPSMove = true;
}
else if(compass == CompassSource.PSMove)
{
posePSMove = inputManager.GetMoveWand(compassPSMoveID);
checkPSMove = true;
}
else if(headRotationInput == HeadRotationSource.PSMove)
{
posePSMove = inputManager.GetMoveWand(rotationPSMoveID);
checkPSMove = true;
}
}
// Reset view: Is Razer Hydra used for tracking?
if(headPositionInput == HeadPositionSource.RazerHydra)
{
poseRazer = SixenseInput.GetController(positionRazerID);
checkRazer = true;
}
else if(compass == CompassSource.RazerHydra)
{
poseRazer = SixenseInput.GetController(compassRazerID);
checkRazer = true;
}
else if(headRotationInput == HeadRotationSource.RazerHydra)
{
poseRazer = SixenseInput.GetController(rotationRazerID);
checkRazer = true;
}
// Reset view: Check if reset view button was pressed
if(checkPSMove && posePSMove != null)
{
if(posePSMove.moveButtonWasPressed)
ResetOrientation();
}
if(checkRazer && poseRazer != null && poseRazer.Enabled)
{
if( poseRazer.GetButton(SixenseButtons.BUMPER)
&& poseRazer.GetButtonDown(SixenseButtons.START) )
ResetOrientation();
}
if(Input.GetKeyDown(resetKey))
ResetOrientation();
/* If we are using Razer Hydra and it's attached to a moving object (i.e. the user),
lets calculate the position and rotation of the base station */
if(isRazerBaseMobile) // In the beginning of the method we coupled this to tracker sources
{
// Adjust hydraBasePositionOffset and hydraBaseRotationOffset if BUMPER button is down
if(headPositionInput == HeadPositionSource.RazerHydra)
poseRazer = SixenseInput.GetController(positionRazerID);
else if(headRotationInput == HeadRotationSource.RazerHydra)
poseRazer = SixenseInput.GetController(rotationRazerID);
else if(compass == CompassSource.RazerHydra)
poseRazer = SixenseInput.GetController(compassRazerID);
if(poseRazer != null && poseRazer.Enabled && poseRazer.GetButton(SixenseButtons.BUMPER))
{
if(Mathf.Abs(poseRazer.JoystickX) > 0.1f)
{
if(mobileRazerBase == RazerHydraBase.Kinect)
hydraBasePositionOffsetKinect.x += 0.5f*deltaT*poseRazer.JoystickX;
}
if(Mathf.Abs(poseRazer.JoystickY) > 0.1f)
{
if(mobileRazerBase == RazerHydraBase.Kinect)
hydraBasePositionOffsetKinect.y += 0.5f*deltaT*poseRazer.JoystickY;
}
if(poseRazer.GetButton(SixenseButtons.THREE))
{
if(mobileRazerBase == RazerHydraBase.Kinect)
hydraBaseRotationOffsetKinect.x += 60*deltaT;
}
if(poseRazer.GetButton(SixenseButtons.ONE))
{
if(mobileRazerBase == RazerHydraBase.Kinect)
hydraBaseRotationOffsetKinect.x -= 60*deltaT;
}
}
switch(mobileRazerBase)
{
case RazerHydraBase.Kinect:
if (skeletonManager)
{
jointData = skeletonManager.GetJointData(hydraBaseJoint, hydraBaseKinectPlayerID);
if( skeletonManager.skeletons[hydraBaseKinectPlayerID].isTracking
&& jointData != null)
{
filterHydraBasePose = filterHydraBasePoseKinect;
hydraBasePositionCovariance = hydraBasePositionCovarianceKinect
+ Mathf.Clamp01(1.0f - jointData.positionConfidence)*2000;
hydraBaseRotationCovariance = hydraBaseRotationCovarianceKinect;
if( inferBaseRotationFromRotationTrackerKinect
&& headRotationInput != HeadRotationSource.RazerHydra)
{
// Assuming that poseRazer is attached to Rotation Tracker
if(poseRazer != null && poseRazer.Enabled)
{
// Offset-adjusted Razer Hydra rotation in offset-adjusted base station coordinate
// system: Rotation from base to Razer Hydra
tempLocalRotation = Quaternion.Euler(hydraBaseRotationOffsetKinect)
* poseRazer.Rotation
* Quaternion.Inverse(Quaternion.Euler(hydraAtRotationTrackerOffset));
// Subtract above rotation from Rotation Tracker's rotation (drift corrected, if enabled)
hydraTempRotation = localRotation * Quaternion.Inverse(tempLocalRotation);
// Get yaw rotation of above, result is the base station's yaw in Unity world coordinates
hydraTempRotation = Quaternion.Euler(0, hydraTempRotation.eulerAngles.y, 0) ;
// hydraTempVector will become hydraBasePosition after filtering
hydraTempVector = jointData.position + hydraTempRotation * hydraBasePositionOffsetKinect;
// Apply base station offset to hydraTempRotation, that will become hydraBaseRotation
hydraTempRotation = hydraTempRotation * Quaternion.Euler(hydraBaseRotationOffsetKinect);
}
}
else
{
hydraTempVector = jointData.position + jointData.rotation * hydraBasePositionOffsetKinect;
hydraTempRotation = jointData.rotation * Quaternion.Euler(hydraBaseRotationOffsetKinect);
hydraBaseRotationCovariance += Mathf.Clamp01(1.0f - jointData.rotationConfidence)*2000;
}
}
}
break;
case RazerHydraBase.InputTransform:
if(hydraBaseInput)
{
filterHydraBasePose = filterHydraBasePoseTransform;
hydraBasePositionCovariance = hydraBasePositionCovarianceTransform;
hydraBaseRotationCovariance = hydraBaseRotationCovarianceTransform;
if( inferBaseRotationFromRotationTrackerTransform
&& headRotationInput != HeadRotationSource.RazerHydra)
{
// Assuming that poseRazer is attached to Rotation Tracker
if(poseRazer != null && poseRazer.Enabled)
{
// Offset-adjusted Razer Hydra rotation in base station coordinate
// system: Rotation from base to Razer Hydra
tempLocalRotation = poseRazer.Rotation
* Quaternion.Inverse(Quaternion.Euler(hydraAtRotationTrackerOffset));
// Subtract above rotation from Rotation Tracker's rotation (drift corrected, if enabled)
hydraTempRotation = localRotation * Quaternion.Inverse(tempLocalRotation);
// Get yaw rotation of above, result is the base station's yaw in Unity world coordinates
hydraTempRotation = Quaternion.Euler(0, hydraTempRotation.eulerAngles.y, 0) ;
// hydraTempVector will become hydraBasePosition after filtering
hydraTempVector = hydraBaseInput.position;
}
}
else
{
hydraTempVector = hydraBaseInput.position;
hydraTempRotation = hydraBaseInput.rotation;
}
}
break;
default:
filterHydraBasePose = false;
break;
}
if(filterHydraBasePose)
{
measuredPos[0] = hydraTempVector.x;
measuredPos[1] = hydraTempVector.y;
measuredPos[2] = hydraTempVector.z;
hydraBaseFilterPos.setR(deltaT * hydraBasePositionCovariance);
hydraBaseFilterPos.predict();
hydraBaseFilterPos.update(measuredPos);
filteredPos = hydraBaseFilterPos.getState();
hydraBasePosition = new Vector3( (float) filteredPos[0],
(float) filteredPos[1],
(float) filteredPos[2] );
}
else
hydraBasePosition = hydraTempVector;
// float normalizedT = Mathf.Clamp01(deltaT * 5);
// if(normalizedT != 0)
// hydraBasePosition = Vector3.Lerp(hydraBasePosition, hydraTempVector, normalizedT );
if(filterHydraBasePose)
{
// measuredRot[0] = hydraTempRotation.x;
// measuredRot[1] = hydraTempRotation.y;
// measuredRot[2] = hydraTempRotation.z;
// measuredRot[3] = hydraTempRotation.w;
// hydraBaseFilterRot.setR(deltaT * hydraBaseRotationCovariance);
// hydraBaseFilterRot.predict();
// hydraBaseFilterRot.update(measuredRot);
// filteredRot = hydraBaseFilterRot.getState();
// hydraBaseRotation = new Quaternion( (float) filteredRot[0], (float) filteredRot[1],
// (float) filteredRot[2], (float) filteredRot[3] );
hydraBaseKalmanRot.rotationNoiseCovariance = hydraBaseRotationCovariance;
hydraBaseRotation = hydraBaseKalmanRot.Update(hydraTempRotation, deltaT);
}
else
hydraBaseRotation = hydraTempRotation;
// normalizedT = Mathf.Clamp01(deltaT * 5);
// if(normalizedT != 0)
// hydraBaseRotation = Quaternion.Lerp(hydraBaseRotation, hydraTempRotation, normalizedT);
}
else
{
hydraBasePosition = new Vector3(0, 0, 0);
hydraBaseRotation = Quaternion.identity;
}
switch(headPositionInput)
{
case HeadPositionSource.Kinect:
if ( skeletonManager
&& skeletonManager.skeletons[positionPlayerID].torso.positionConfidence >= 1) // Most stable joint is torso
{
filterPosition = filterPositionKinect;
positionNoiseCovariance = positionNoiseCovarianceKinect;
jointData = skeletonManager.GetJointData(positionJoint, positionPlayerID);
if(jointData != null)
measuredHeadPosition = jointData.position // Fix for Kinect2: below takes rotation from torso
- skeletonManager.skeletons[positionPlayerID].torso.rotation
* Quaternion.Inverse(Quaternion.Euler(rotationOffsetKinect)) * positionOffsetKinect;
}
break;
case HeadPositionSource.PSMove:
if (inputManager)
{
posePSMove = inputManager.GetMoveWand(positionPSMoveID);
if(posePSMove)
{
filterPosition = filterPositionPSMove;
positionNoiseCovariance = positionNoiseCovariancePSMove;
measuredHeadPosition = posePSMove.localPosition
- posePSMove.localRotation * Quaternion.Inverse(Quaternion.Euler(rotationOffsetPSMove))
* positionOffsetPSMove;
}
}
break;
case HeadPositionSource.RazerHydra:
poseRazer = SixenseInput.GetController(positionRazerID);
if(poseRazer != null && poseRazer.Enabled)
{
filterPosition = filterPositionHydra;
positionNoiseCovariance = positionNoiseCovarianceHydra;
measuredHeadPosition = new Vector3( poseRazer.Position.x * sensitivity.x,
poseRazer.Position.y * sensitivity.y,
poseRazer.Position.z * sensitivity.z )
- poseRazer.Rotation * Quaternion.Inverse(Quaternion.Euler(rotationOffsetHydra))
* positionOffsetHydra;
if(isRazerBaseMobile)
measuredHeadPosition = hydraBasePosition + hydraBaseRotation*measuredHeadPosition;
}
break;
case HeadPositionSource.InputTransform:
if(positionInput)
{
filterPosition = filterPositionTransform;
positionNoiseCovariance = positionNoiseCovarianceTransform;
measuredHeadPosition = positionInput.position;
}
break;
case HeadPositionSource.None:
filterPosition = false;
break;
}
if (filterPosition)
{
measuredPos[0] = measuredHeadPosition.x;
measuredPos[1] = measuredHeadPosition.y;
measuredPos[2] = measuredHeadPosition.z;
filterPos.setR(deltaT * positionNoiseCovariance);
filterPos.predict();
filterPos.update(measuredPos);
filteredPos = filterPos.getState();
localPosition = new Vector3((float) filteredPos[0], (float) filteredPos[1], (float) filteredPos[2]);
transform.localPosition = localPosition;
}
else
{
//if((localPosition - measuredHeadPosition).magnitude > 0.3f)
// Debug.LogError("aa " + (localPosition - measuredHeadPosition).magnitude + "locR "
/// + localRotation + "bPos " + hydraBasePosition+ "bRot " + hydraBaseRotation);
//else print ("ok " + (localPosition - measuredHeadPosition).magnitude + "locR "
// + localRotation + "bPos " + hydraBasePosition+ "bRot " + hydraBaseRotation);
localPosition = measuredHeadPosition;
transform.localPosition = measuredHeadPosition;
}
// Determine whether rotation source is Oculus Rift or some other device
if(useOculusRiftRotation)
{
if(OVRDevice.IsSensorPresent(oculusID))
{
if(!OVRDevice.GetOrientation(oculusID, ref tempLocalRotation))
tempLocalRotation = Quaternion.identity;
}
}
else
{
switch(headRotationInput)
{
//case HeadRotationSource.OculusRift:
// In this case rotation is applied by OVRCameraController which should be parented
// under this GameObject
// break;
case HeadRotationSource.Kinect:
if ( skeletonManager
&& skeletonManager.skeletons[rotationPlayerID].torso.rotationConfidence >= 1)
{
filterRotation = filterRotationKinect;
rotationNoiseCovariance = rotationNoiseCovarianceKinect;
jointData = skeletonManager.GetJointData(rotationJoint, rotationPlayerID);
// Most stable joint:
if(jointData != null && jointData.rotationConfidence >= 1)
measuredHeadRotation = jointData.rotation * Quaternion.Inverse(Quaternion.Euler(rotationOffsetKinect));
}
break;
case HeadRotationSource.PSMove:
if (inputManager)
{
posePSMove = inputManager.GetMoveWand(rotationPSMoveID);
if(posePSMove)
{
filterRotation = filterRotationPSMove;
rotationNoiseCovariance = rotationNoiseCovariancePSMove;
measuredHeadRotation = posePSMove.localRotation * Quaternion.Inverse(Quaternion.Euler(rotationOffsetPSMove));
}
}
break;
case HeadRotationSource.RazerHydra:
poseRazer = SixenseInput.GetController(rotationRazerID);
if(poseRazer != null && poseRazer.Enabled)
{
filterRotation = filterRotationHydra;
rotationNoiseCovariance = rotationNoiseCovarianceHydra;
measuredHeadRotation = poseRazer.Rotation * Quaternion.Inverse(Quaternion.Euler(rotationOffsetHydra));
if(isRazerBaseMobile)
measuredHeadRotation = hydraBaseRotation * measuredHeadRotation;
}
break;
case HeadRotationSource.InputTransform:
if(rotationInput)
{
filterRotation = filterRotationTransform;
rotationNoiseCovariance = rotationNoiseCovarianceTransform;
measuredHeadRotation = rotationInput.rotation;
}
break;
case HeadRotationSource.None:
filterRotation = false;
break;
}
if (filterRotation)
{
// measuredRot[0] = measuredHeadRotation.x;
// measuredRot[1] = measuredHeadRotation.y;
// measuredRot[2] = measuredHeadRotation.z;
// measuredRot[3] = measuredHeadRotation.w;
// filterRot.setR(deltaT * rotationNoiseCovariance);
// filterRot.predict();
// filterRot.update(measuredRot);
// filteredRot = filterRot.getState();
// tempLocalRotation = new Quaternion( (float) filteredRot[0], (float) filteredRot[1],
// (float) filteredRot[2], (float) filteredRot[3] );
filterRot.rotationNoiseCovariance = rotationNoiseCovariance;
tempLocalRotation = filterRot.Update(measuredHeadRotation, deltaT);
}
else
tempLocalRotation = measuredHeadRotation;
}
rawRotation = tempLocalRotation;
// Do yaw drift correction for rotation source if that option is enabled and necessary
if( !externalDriftCorrection
|| compass == CompassSource.None )
{
localRotation = rawRotation;
transform.localRotation = rawRotation;
}
else
{
localRotation = driftCorrectedRotation(tempLocalRotation, deltaT);
transform.localRotation = localRotation;
}
}
void Awake()
{
coordinateSystem = MonoBehaviour.FindObjectOfType(typeof(RUISCoordinateSystem)) as RUISCoordinateSystem;
localPosition = Vector3.zero;
localRotation = Quaternion.identity;
rawRotation = Quaternion.identity;
filterPos = new KalmanFilter();
filterPos.initialize(3,3);
filterPos.skipIdenticalMeasurements = true;
// filterRot = new KalmanFilter();
// filterRot.initialize(4,4);
// Mobile Razer Hydra base filtering
hydraBaseFilterPos = new KalmanFilter();
hydraBaseFilterPos.initialize(3,3);
hydraBaseFilterPos.skipIdenticalMeasurements = true;
// hydraBaseFilterRot = new KalmanFilter();
// hydraBaseFilterRot.initialize(4,4);
filterRot.skipIdenticalMeasurements = true;
// Yaw Drift Corrector invocations in Awake()
filterDrift = new KalmanFilter();
filterDrift.initialize(2,2);
transform.localPosition = defaultPosition;
eyeCenterPosition = defaultPosition;
measuredHeadPosition = defaultPosition;
hydraBasePosition = new Vector3(0, 0, 0);
hydraBaseRotation = Quaternion.identity;
ovrCameraRig = GetComponentInChildren<OVRCameraRig>();
if(ovrCameraRig != null && OVRManager.display != null && OVRManager.display.isPresent)
{
useOculusRiftRotation = true;
}
else
{
useOculusRiftRotation = false;
}
// Enforce rotation settings if rotation source is set to be same as position source
if (!pickRotationSource)
{
switch (headPositionInput)
{
case HeadPositionSource.Kinect1:
{
headRotationInput = HeadRotationSource.Kinect1;
rotationPlayerID = positionPlayerID;
rotationJoint = positionJoint;
break;
}
case HeadPositionSource.Kinect2:
{
headRotationInput = HeadRotationSource.Kinect2;
rotationPlayerID = positionPlayerID;
rotationJoint = positionJoint;
break;
}
case HeadPositionSource.PSMove:
{
headRotationInput = HeadRotationSource.PSMove;
rotationPSMoveID = positionPSMoveID;
break;
}
case HeadPositionSource.RazerHydra:
{
headRotationInput = HeadRotationSource.RazerHydra;
rotationRazerID = positionRazerID;
break;
}
case HeadPositionSource.InputTransform:
{
headRotationInput = HeadRotationSource.InputTransform;
rotationInput = positionInput;
break;
}
case HeadPositionSource.None:
{
headRotationInput = HeadRotationSource.None;
break;
}
}
}
filterPosition = false;
}
