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; }