void Update()
{
timeSinceLastUpdate += Time.deltaTime;
if (skeletonManager == null)
{
RUISSkeletonManager.JointData jointData = skeletonManager.GetJointData(jointToFollow, player.kinectPlayerId, player.bodyTrackingDeviceID);
PointData newPoint = new PointData(jointData.position, jointData.rotation, timeSinceLastUpdate, Time.timeSinceLevelLoad, previousPoint);
//remove zero velocities just in case, in order for the speeds not to get polluted by nonexisting data
//if (newPoint.velocity == Vector3.zero) return;
points.Add(newPoint);
previousPoint = newPoint;
Normalize();
MoveToStart();
while (points[points.Count - 1].startTime - points[0].startTime >= bufferLength)
{
points.RemoveAt(0);
}
}
//if (points.Count > bufferSize) points.RemoveAt(0);
InvalidateCaches();
//Debug.Log(averageSpeed);
timeSinceLastUpdate = 0;
}
private void UpdateTransform(ref Transform transformToUpdate, RUISSkeletonManager.JointData jointToGet)
{
if (transformToUpdate == null)
{
return;
}
if (updateJointPositions && jointToGet.positionConfidence >= minimumConfidenceToUpdate)
{
transformToUpdate.localPosition = jointToGet.position - skeletonPosition;
}
if (updateJointRotations && jointToGet.rotationConfidence >= minimumConfidenceToUpdate)
{
if (useHierarchicalModel)
{
Quaternion newRotation = transform.rotation * jointToGet.rotation *
(jointInitialRotations.ContainsKey(transformToUpdate) ? jointInitialRotations[transformToUpdate] : Quaternion.identity);
transformToUpdate.rotation = Quaternion.RotateTowards(transformToUpdate.rotation, newRotation, Time.deltaTime * rotationDamping);
}
else
{
transformToUpdate.localRotation = Quaternion.RotateTowards(transformToUpdate.localRotation, jointToGet.rotation, Time.deltaTime * rotationDamping);
}
}
}
private void ForceUpdatePosition(ref Transform transformToUpdate, RUISSkeletonManager.JointData jointToGet)
{
if (transformToUpdate == null)
{
return;
}
transformToUpdate.position = transform.TransformPoint(jointToGet.position - skeletonPosition);//transform.position + transform.rotation * (jointToGet.position - skeletonPosition);
}
private float UpdateBoneScaling(Transform boneToScale, Transform comparisonBone, RUISSkeletonManager.JointData boneToScaleTracker, RUISSkeletonManager.JointData comparisonBoneTracker, float cumulativeScale)
{
float modelBoneLength = jointInitialDistances[new KeyValuePair <Transform, Transform>(boneToScale, comparisonBone)];
float playerBoneLength = Vector3.Distance(boneToScaleTracker.position, comparisonBoneTracker.position);
float newScale = playerBoneLength / modelBoneLength / cumulativeScale;
boneToScale.localScale = Vector3.MoveTowards(boneToScale.localScale, new Vector3(newScale, newScale, newScale), maxScaleFactor * Time.deltaTime);
return(boneToScale.localScale.x);
}
void Update()
{
if (!skeletonManager || !skeletonManager.skeletons[playerId].isTracking)
{
return;
}
RUISSkeletonManager.JointData jointData = skeletonManager.GetJointData(jointToFollow, playerId);
if (jointData.positionConfidence > minimumConfidenceToUpdate)
{
transform.localPosition = Vector3.Lerp(transform.localPosition, jointData.position, positionSmoothing * Time.deltaTime);
}
if (jointData.rotationConfidence > minimumConfidenceToUpdate)
{
transform.localRotation = Quaternion.Slerp(transform.localRotation, jointData.rotation, rotationSmoothing * Time.deltaTime);
}
}
public void Update()
{
if (!isTracking && skeletonManager.skeletons[bodyTrackingDeviceID, playerId].isTracking)
{
PlayerFound();
}
else if (isTracking && !skeletonManager.skeletons[bodyTrackingDeviceID, playerId].isTracking)
{
PlayerLost();
}
else if (!skeletonManager.skeletons[bodyTrackingDeviceID, playerId].isTracking)
{
return;
}
if (!highlightStartObject && wandSelector.HighlightedObject)
{
highlightStartObject = wandSelector.HighlightedObject;
gestureRecognizer.EnableGesture();
}
else if (!wandSelector.HighlightedObject)
{
highlightStartObject = null;
if (!wandSelector.Selection)
{
gestureRecognizer.DisableGesture();
}
}
visualizerThreshold = Mathf.Clamp01(visualizerThreshold);
RUISSkeletonManager.JointData startData = skeletonManager.GetJointData(wandStart, playerId, bodyTrackingDeviceID);
RUISSkeletonManager.JointData endData = skeletonManager.GetJointData(wandEnd, playerId, bodyTrackingDeviceID);
if (endData.positionConfidence >= 0.5f)
{
// TUUKKA: Original code
// transform.localPosition = endData.position;
//
// if (startData != null && startData.positionConfidence >= 0.5f)
// {
// transform.localRotation = Quaternion.LookRotation(endData.position - startData.position);
// }
// else if (endData.rotationConfidence >= 0.5f)
// {
// transform.localRotation = endData.rotation;
// }
// First calculate local rotation
if (startData != null && startData.positionConfidence >= 0.5f)
{
tempVector = endData.position - startData.position;
if (Vector3.Angle(startData.rotation * Vector3.up, tempVector) > 5)
{
tempRotation = Quaternion.LookRotation(endData.position - startData.position, startData.rotation * Vector3.up);
}
else
{
tempRotation = Quaternion.LookRotation(endData.position - startData.position, startData.rotation * Vector3.right);
}
filteredRotation = rotationFilter.Update(tempRotation, Time.deltaTime); // HACK with kinect2 filtering is done in SkeletonManager
}
// else if (endData.rotationConfidence >= 0.5f)
// {
// tempRotation = endData.rotation;
// filteredRotation = rotationFilter.Update(tempRotation, Time.deltaTime);
// }
if (rigidbody)
{
// TUUKKA:
if (transform.parent)
{
// If the wand has a parent, we need to apply its transformation first
rigidbody.MovePosition(transform.parent.TransformPoint(endData.position));
rigidbody.MoveRotation(transform.parent.rotation * filteredRotation);
}
else
{
rigidbody.MovePosition(endData.position);
rigidbody.MoveRotation(filteredRotation);
}
}
else
{
// If there is no rigidBody, then just change localPosition & localRotation
transform.localPosition = endData.position;
transform.localRotation = filteredRotation;
}
}
}
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;
}
}
private void doYawFiltering(Quaternion driftingOrientation, float deltaT)
{
// If the Rift is HeadRotationSource, we need to apply the yaw correction to it
if(useOculusRiftRotation)
{
if(OVRDevice.IsSensorPresent(oculusID))
{
if(oculusCamController)
{
// In the future OVR SDK oculusCamController will have oculusID?
oculusCamController.SetYRotation(-finalYawDifference.eulerAngles.y);
}
}
}
driftingEuler = driftingOrientation.eulerAngles;
// You can set compassIsPositionTracker to true in a script and it will work as
// expected, but if you return it to false, it doesn't remember what the compass
// was before setting it to true
// if(compassIsPositionTracker)
// {
// if(headPositionInput == HeadPositionSource.None)
// return; // Don't do yaw drift correction in this case
//
// switch(headPositionInput)
// {
// case HeadPositionSource.Kinect:
// compass = CompassSource.Kinect;
// compassPlayerID = positionPlayerID;
// compassJoint = positionJoint;
// break;
// case HeadPositionSource.PSMove:
// compass = CompassSource.PSMove;
// compassPSMoveID = positionPSMoveID;
// break;
// case HeadPositionSource.RazerHydra:
// compass = CompassSource.RazerHydra;
// compassRazerID = positionRazerID;
// break;
// case HeadPositionSource.InputTransform:
// compass = CompassSource.InputTransform;
// compassTransform = positionInput;
// break;
// }
// }
float driftCorrectionRate = 0.1f;
switch(compass)
{
case CompassSource.Kinect:
if (!skeletonManager || !skeletonManager.skeletons[compassPlayerID].isTracking)
{
break;
}
else
{
compassData = skeletonManager.GetJointData(compassJoint, compassPlayerID);
// First check for high confidence value
if (compassData != null && compassData.rotationConfidence >= 1.0f)
{
driftCorrectionRate = driftCorrectionRateKinect;
updateDifferenceKalman( (compassData.rotation
* Quaternion.Inverse(Quaternion.Euler(compassRotationOffsetKinect))).eulerAngles,
driftingEuler, deltaT );
}
}
break;
case CompassSource.PSMove:
if (inputManager)
{
compassPSMove = inputManager.GetMoveWand(compassPSMoveID);
if(compassPSMove)
{
driftCorrectionRate = driftCorrectionRatePSMove;
updateDifferenceKalman( (compassPSMove.localRotation
* Quaternion.Inverse(Quaternion.Euler(compassRotationOffsetPSMove))).eulerAngles,
driftingEuler, deltaT );
}
}
break;
case CompassSource.RazerHydra:
compassRazer = SixenseInput.GetController(compassRazerID);
if(compassRazer != null && compassRazer.Enabled)
{
driftCorrectionRate = driftCorrectionRateHydra;
if(isRazerBaseMobile)
updateDifferenceKalman((hydraBaseRotation * compassRazer.Rotation
* Quaternion.Inverse(Quaternion.Euler(compassRotationOffsetHydra))).eulerAngles,
driftingEuler, deltaT );
else
updateDifferenceKalman( (compassRazer.Rotation
* Quaternion.Inverse(Quaternion.Euler(compassRotationOffsetHydra))).eulerAngles,
driftingEuler, deltaT );
}
break;
case CompassSource.InputTransform:
if(compassTransform != null)
{
driftCorrectionRate = driftCorrectionRateTransform;
updateDifferenceKalman( compassTransform.rotation.eulerAngles,
driftingEuler, deltaT );
}
break;
}
float normalizedT = Mathf.Clamp01(deltaT * driftCorrectionRate);
if(normalizedT != 0)
finalYawDifference = Quaternion.Lerp(finalYawDifference, filteredYawDifference,
normalizedT );
// TODO: REMOVE THIS ***
// if(finalYawDifference.x*finalYawDifference.x + finalYawDifference.y*finalYawDifference.y
// + finalYawDifference.z*finalYawDifference.z + finalYawDifference.w*finalYawDifference.w < 0.3)
// Debug.LogError("LERP:ing quaternions was a bad idea: " + finalYawDifference);
if(enableVisualizers)
{
if(driftingDirectionVisualizer != null)
driftingDirectionVisualizer.transform.rotation = driftingOrientation;
if(correctedDirectionVisualizer != null)
correctedDirectionVisualizer.transform.rotation = Quaternion.Euler(
new Vector3(driftingEuler.x,
(360 + driftingEuler.y
- finalYawDifference.eulerAngles.y)%360,
driftingEuler.z));
if(driftVisualizerPosition != null)
{
if(driftingDirectionVisualizer != null)
driftingDirectionVisualizer.transform.position = driftVisualizerPosition.position;
if(compassDirectionVisualizer != null)
compassDirectionVisualizer.transform.position = driftVisualizerPosition.position;
if(correctedDirectionVisualizer != null)
correctedDirectionVisualizer.transform.position = driftVisualizerPosition.position;
}
}
}
/// <summary>
/// Sets the Rotation Tracker's rotation offset (euler angles) to the source
/// rotation's current value. The resulting rotation offset will be correct
/// if the tracked object (e.g. head) is oriented along Unity world coordinates,
/// i.e. the tracked object is "looking" into +Z-direction while its "top" is
/// pointing into +Y-direction.
/// </summary>
public Vector3 CalibrateRotationOffset()
{
filterRot.Reset();
switch(headRotationInput)
{
case HeadRotationSource.Kinect:
if (skeletonManager)
{
jointData = skeletonManager.GetJointData(rotationJoint, rotationPlayerID);
if(jointData != null)
{
rotationOffsetKinect = jointData.rotation.eulerAngles;
return rotationOffsetKinect;
}
}
break;
case HeadRotationSource.PSMove:
if (inputManager)
{
posePSMove = inputManager.GetMoveWand(rotationPSMoveID);
if(posePSMove)
{
rotationOffsetPSMove = posePSMove.localRotation.eulerAngles;
return rotationOffsetPSMove;
}
}
break;
case HeadRotationSource.RazerHydra:
poseRazer = SixenseInput.GetController(rotationRazerID);
if(poseRazer != null && poseRazer.Enabled)
{
rotationOffsetHydra = poseRazer.Rotation.eulerAngles;
return rotationOffsetHydra;
}
break;
}
return Vector3.zero;
}
private void doYawFiltering(float deltaT)
{
switch (driftingSensor)
{
case DriftingRotation.OculusRift:
if (OVRDevice.IsSensorPresent(oculusID))
{
OVRDevice.GetOrientation(oculusID, ref driftingRot);
if (oculusCamController)
{
// In the future OVR SDK oculusCamController will have oculusID?
oculusCamController.SetYRotation(-finalYawDifference.eulerAngles.y);
}
}
break;
case DriftingRotation.RazerHydra:
// TODO
//driftingRot = hydraRotation;
break;
case DriftingRotation.InputTransform:
if (driftingTransform)
{
driftingRot = driftingTransform.rotation;
}
break;
}
if (driftingDirectionVisualizer != null)
{
driftingDirectionVisualizer.transform.rotation = driftingRot;
}
driftingEuler = driftingRot.eulerAngles;
switch (compass)
{
case CompassSource.Kinect:
if (!skeletonManager || !skeletonManager.skeletons[kinectPlayerID].isTracking)
{
break;
}
else
{
compassData = skeletonManager.GetJointData(compassJoint, kinectPlayerID);
// First check for high confidence value
if (compassData != null && compassData.rotationConfidence >= 1.0f)
{
updateDifferenceKalman(compassData.rotation.eulerAngles,
driftingEuler, deltaT);
}
}
break;
case CompassSource.PSMove:
if (inputManager)
{
compassMove = inputManager.GetMoveWand(PSMoveID);
if (compassMove)
{
updateDifferenceKalman(compassMove.localRotation.eulerAngles,
driftingEuler, deltaT);
}
}
break;
case CompassSource.InputTransform:
if (compassTransform != null)
{
updateDifferenceKalman(compassTransform.rotation.eulerAngles,
driftingEuler, deltaT);
}
break;
}
float normalizedT = Mathf.Clamp01(deltaT * driftCorrectionRate);
if (normalizedT != 0)
{
finalYawDifference = Quaternion.Lerp(finalYawDifference, filteredYawDifference,
normalizedT);
}
if (correctedDirectionVisualizer != null)
{
correctedDirectionVisualizer.transform.rotation = Quaternion.Euler(
new Vector3(driftingEuler.x,
(360 + driftingEuler.y
- finalYawDifference.eulerAngles.y) % 360,
driftingEuler.z));
}
//driftingRotation*Quaternion.Inverse(finalDifference);
if (correctedDirectionVisualizer != null && driftVisualizerPosition != null)
{
correctedDirectionVisualizer.transform.position = driftVisualizerPosition.position;
}
}
private void doYawFiltering(float deltaT)
{
switch(driftingSensor)
{
case DriftingRotation.OculusRift:
if(OVRDevice.IsSensorPresent(oculusID))
{
OVRDevice.GetOrientation(oculusID, ref driftingRot);
if(oculusCamController)
{
// In the future OVR SDK oculusCamController will have oculusID?
oculusCamController.SetYRotation(-finalYawDifference.eulerAngles.y);
}
}
break;
case DriftingRotation.RazerHydra:
// TODO
//driftingRot = hydraRotation;
break;
case DriftingRotation.InputTransform:
if(driftingTransform)
{
driftingRot = driftingTransform.rotation;
}
break;
}
if(driftingDirectionVisualizer != null)
driftingDirectionVisualizer.transform.rotation = driftingRot;
driftingEuler = driftingRot.eulerAngles;
switch(compass)
{
case CompassSource.Kinect:
if (!skeletonManager || !skeletonManager.skeletons[kinectPlayerID].isTracking)
{
break;
}
else
{
compassData = skeletonManager.GetJointData(compassJoint, kinectPlayerID);
// First check for high confidence value
if (compassData != null && compassData.rotationConfidence >= 1.0f)
{
updateDifferenceKalman( compassData.rotation.eulerAngles,
driftingEuler, deltaT );
}
}
break;
case CompassSource.PSMove:
if (inputManager)
{
compassMove = inputManager.GetMoveWand(PSMoveID);
if(compassMove)
{
updateDifferenceKalman( compassMove.localRotation.eulerAngles,
driftingEuler, deltaT );
}
}
break;
case CompassSource.InputTransform:
if(compassTransform != null)
updateDifferenceKalman( compassTransform.rotation.eulerAngles,
driftingEuler, deltaT );
break;
}
float normalizedT = Mathf.Clamp01(deltaT * driftCorrectionRate);
if(normalizedT != 0)
finalYawDifference = Quaternion.Lerp(finalYawDifference, filteredYawDifference,
normalizedT );
if(correctedDirectionVisualizer != null)
correctedDirectionVisualizer.transform.rotation = Quaternion.Euler(
new Vector3(driftingEuler.x,
(360 + driftingEuler.y
- finalYawDifference.eulerAngles.y)%360,
driftingEuler.z));
//driftingRotation*Quaternion.Inverse(finalDifference);
if(correctedDirectionVisualizer != null && driftVisualizerPosition != null)
correctedDirectionVisualizer.transform.position = driftVisualizerPosition.position;
}
