public void Disconnect() { if (Ble != null) { //unsubscribe Ble.ConnectionStatusChanged -= StatusChanged; //sources(services) AudioLevelSource.Dispose(); AccGyroMagSource.Dispose(); ActivityRecognitionSource.Dispose(); BatteryStatusSource.Dispose(); CarryPositionSource.Dispose(); CompassSource.Dispose(); EnvironmentalSource.Dispose(); GestureRecognitionSource.Dispose(); LEDStateConfigSource.Dispose(); LightSensorSource.Dispose(); OrientationSource.Dispose(); ProximitySource.Dispose(); SmokeSensorSource.Dispose(); LEDStateConfigSource.Dispose(); UltraVioletSource.Dispose(); //device Ble.Dispose(); GC.Collect(); GC.WaitForPendingFinalizers(); Ble = null; } }
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; } }