public Vector3 TransformDirection(Vector3 directionInCharacterCoordinates)
{
Vector3 characterForward = Vector3.forward;
switch (characterPivotType)
{
case CharacterPivotType.KinectHead:
if (skeletonManager != null && skeletonManager.skeletons[kinectPlayerId] != null)
{
characterForward = skeletonManager.skeletons[kinectPlayerId].head.rotation * Vector3.forward;
}
else
{
characterForward = Vector3.forward;
}
break;
case CharacterPivotType.KinectTorso:
if (skeletonManager != null && skeletonManager.skeletons[kinectPlayerId] != null)
{
characterForward = skeletonManager.skeletons[kinectPlayerId].torso.rotation * Vector3.forward;
}
else
{
characterForward = Vector3.forward;
}
break;
case CharacterPivotType.MoveController:
{
RUISPSMoveWand psmove = inputManager.GetMoveWand(moveControllerId);
if (psmove != null)
{
characterForward = psmove.localRotation * Vector3.forward;
}
}
break;
}
if (ignorePitchAndRoll)
{
characterForward.y = 0;
characterForward.Normalize();
}
characterForward = transform.TransformDirection(characterForward);
return(Quaternion.LookRotation(characterForward, transform.up) * directionInCharacterCoordinates);
}
void Start()
{
RUISInputManager inputManager = FindObjectOfType(typeof(RUISInputManager)) as RUISInputManager;
if(inputManager)
{
RUISPSMoveWand[] moveWands = inputManager.GetComponentsInChildren<RUISPSMoveWand>();
foreach(RUISPSMoveWand wand in moveWands)
{
if(moveID == wand.controllerId)
moveWand = wand;
}
}
rigidBody = this.gameObject.GetComponent<Rigidbody>();
}
void Start()
{
RUISInputManager inputManager = FindObjectOfType(typeof(RUISInputManager)) as RUISInputManager;
if (inputManager)
{
RUISPSMoveWand[] moveWands = inputManager.GetComponentsInChildren <RUISPSMoveWand>();
foreach (RUISPSMoveWand wand in moveWands)
{
if (moveID == wand.controllerId)
{
moveWand = wand;
}
}
}
rigidBody = this.gameObject.GetComponent <Rigidbody>();
}
private void DoCalibrating()
{
statusText.text = string.Format("Calibrating... {0}/{1} samples taken.", numberOfSamplesTaken, minNumberOfSamplesToTake);
TakeSample();
if (numberOfSamplesTaken >= minNumberOfSamplesToTake)
{
statusText.text = "All samples taken.\nProcessing...";
currentState = State.CalibrationReview;
CalculateTransformation();
moveController.gameObject.SetActive(true);
float distance = 0;
Vector3 error = Vector3.zero;
List <float> errorMagnitudes = new List <float>();
for (int i = 0; i < calibrationSpheres.Count; i++)
{
//Destroy(sphere);
GameObject sphere = calibrationSpheres[i];
Vector3 cubePosition = coordinateSystem.ConvertMovePosition(rawPSMoveSamples[i]);
GameObject cube = Instantiate(calibrationCube, cubePosition, Quaternion.identity) as GameObject;
cube.GetComponent <RUISMoveCalibrationVisualizer>().kinectCalibrationSphere = sphere;
distance += Vector3.Distance(sphere.transform.position, cubePosition);
errorMagnitudes.Add(distance);
error += cubePosition - sphere.transform.position;
sphere.transform.parent = calibrationReviewGameObjects.transform;
cube.transform.parent = calibrationReviewGameObjects.transform;
}
totalErrorDistance = distance;
averageError = distance / calibrationSpheres.Count;
SetCalibrationReviewShowing(true);
RUISPSMoveWand controller = FindObjectOfType(typeof(RUISPSMoveWand)) as RUISPSMoveWand;
controller.controllerId = calibratingPSMoveControllerId;
psEyeModelObject.transform.position = coordinateSystem.ConvertMovePosition(Vector3.zero);
}
}
private IEnumerator DelayedWandActivation()
{
yield return(new WaitForSeconds(delayTime));
if (enablePSMove && psMoveWrapper.isConnected)
{
string activated = "";
string leftDisabled = "";
foreach (GameObject moveWand in disabledWands)
{
RUISPSMoveWand moveWandScript = moveWand.GetComponent <RUISPSMoveWand>();
if (psMoveWrapper.moveConnected[moveWandScript.controllerId])
{
moveWand.SetActive(true);
if (activated.Length > 0)
{
activated += ", ";
}
activated += moveWand.name;
}
else
{
if (leftDisabled.Length > 0)
{
leftDisabled += ", ";
}
leftDisabled += moveWand.name;
}
}
string report = "DELAYED CONTROLLER ACTIVATION REPORT: ";
if (activated.Length > 0)
{
report += "Following GameObjects were re-activated: " + activated + ". ";
}
if (leftDisabled.Length > 0)
{
report += "Following GameObjects will stay inactive because the controllers associated to them "
+ "are not connected: " + leftDisabled + ".";
}
Debug.Log(report);
}
}
void LateUpdate()
{
// If a custom skeleton tracking source is used, save its data into skeletonManager (which is a little
// topsy turvy) so we can utilize same code as we did with Kinect 1 and 2
if(bodyTrackingDevice == bodyTrackingDeviceType.GenericMotionTracker)
{
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].isTracking = true;
if(customRoot) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].root.rotation = customRoot.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].root.position = customRoot.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].root.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].root.rotationConfidence = 1;
}
if(customHead) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].head.rotation = customHead.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].head.position = customHead.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].head.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].head.rotationConfidence = 1;
}
if(customNeck) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].neck.rotation = customNeck.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].neck.position = customNeck.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].neck.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].neck.rotationConfidence = 1;
}
if(customTorso) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].torso.rotation = customTorso.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].torso.position = customTorso.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].torso.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].torso.rotationConfidence = 1;
}
if(customRightShoulder) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightShoulder.rotation = customRightShoulder.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightShoulder.position = customRightShoulder.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightShoulder.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightShoulder.rotationConfidence = 1;
}
if(customLeftShoulder) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftShoulder.rotation = customLeftShoulder.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftShoulder.position = customLeftShoulder.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftShoulder.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftShoulder.rotationConfidence = 1;
}
if(customRightElbow) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightElbow.rotation = customRightElbow.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightElbow.position = customRightElbow.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightElbow.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightElbow.rotationConfidence = 1;
}
if(customLeftElbow) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftElbow.rotation = customLeftElbow.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftElbow.position = customLeftElbow.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftElbow.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftElbow.rotationConfidence = 1;
}
if(customRightHand) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightHand.rotation = customRightHand.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightHand.position = customRightHand.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightHand.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightHand.rotationConfidence = 1;
}
if(customLeftHand) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftHand.rotation = customLeftHand.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftHand.position = customLeftHand.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftHand.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftHand.rotationConfidence = 1;
}
if(customRightHip) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightHip.rotation = customRightHip.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightHip.position = customRightHip.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightHip.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightHip.rotationConfidence = 1;
}
if(customLeftHip) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftHip.rotation = customLeftHip.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftHip.position = customLeftHip.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftHip.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftHip.rotationConfidence = 1;
}
if(customRightKnee) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightKnee.rotation = customRightKnee.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightKnee.position = customRightKnee.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightKnee.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightKnee.rotationConfidence = 1;
}
if(customLeftKnee) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftKnee.rotation = customLeftKnee.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftKnee.position = customLeftKnee.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftKnee.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftKnee.rotationConfidence = 1;
}
if(customRightFoot) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightFoot.rotation = customRightFoot.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightFoot.position = customRightFoot.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightFoot.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightFoot.rotationConfidence = 1;
}
if(customLeftFoot) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftFoot.rotation = customLeftFoot.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftFoot.position = customLeftFoot.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftFoot.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftFoot.rotationConfidence = 1;
}
if(customRightThumb) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightThumb.rotation = customRightThumb.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightThumb.position = customRightThumb.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightThumb.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightThumb.rotationConfidence = 1;
}
if(customLeftThumb) {
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftThumb.rotation = customLeftThumb.rotation;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftThumb.position = customLeftThumb.position;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftThumb.positionConfidence = 1;
skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftThumb.rotationConfidence = 1;
}
}
// Update skeleton based on data fetched from skeletonManager
if ( skeletonManager != null && skeletonManager.skeletons [bodyTrackingDeviceID, playerId] != null
&& skeletonManager.skeletons [bodyTrackingDeviceID, playerId].isTracking)
{
// if(bodyTrackingDeviceID == RUISSkeletonManager.kinect2SensorID && !skeletonManager.isNewKinect2Frame)
// return;
float maxAngularVelocity;
// if(bodyTrackingDeviceID == RUISSkeletonManager.kinect2SensorID)
// maxAngularVelocity = skeletonManager.kinect2FrameDeltaT * rotationDamping;
// else
maxAngularVelocity = Time.deltaTime * rotationDamping;
// Obtained new body tracking data. TODO test that Kinect 1 still works
// if(bodyTrackingDeviceID != RUISSkeletonManager.kinect2SensorID || skeletonManager.isNewKinect2Frame)
{
UpdateSkeletonPosition ();
UpdateTransform (ref torso, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].torso, maxAngularVelocity);
UpdateTransform (ref head, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].head, maxAngularVelocity);
}
if(oculusRotatesHead && OVRManager.display != null)
{
if(coordinateSystem)
{
Quaternion oculusRotation = Quaternion.identity;
if(coordinateSystem.applyToRootCoordinates)
{
if(ovrHmdVersion == Ovr.HmdType.DK1 || ovrHmdVersion == Ovr.HmdType.DKHD)
oculusRotation = coordinateSystem.GetOculusRiftOrientationRaw();
else
oculusRotation = coordinateSystem.ConvertRotation(Quaternion.Inverse(coordinateSystem.GetOculusCameraOrientationRaw())
* coordinateSystem.GetOculusRiftOrientationRaw(), RUISDevice.Oculus_DK2);
}
else if(OVRManager.display != null)
oculusRotation = OVRManager.display.GetHeadPose().orientation;
if (useHierarchicalModel)
head.rotation = transform.rotation * oculusRotation /*skeletonManager.skeletons [bodyTrackingDeviceID, playerId].head.rotation*/ *
(jointInitialRotations.ContainsKey(head) ? jointInitialRotations[head] : Quaternion.identity);
else
head.localRotation = oculusRotation; //skeletonManager.skeletons [bodyTrackingDeviceID, playerId].head;
}
}
// Obtained new body tracking data. TODO test that Kinect 1 still works
// if(bodyTrackingDeviceID != RUISSkeletonManager.kinect2SensorID || skeletonManager.isNewKinect2Frame)
{
UpdateTransform (ref leftShoulder, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftShoulder, maxAngularVelocity);
UpdateTransform (ref rightShoulder, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightShoulder, maxAngularVelocity);
if(trackWrist || !useHierarchicalModel)
{
UpdateTransform (ref leftHand, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftHand, 2*maxAngularVelocity);
UpdateTransform (ref rightHand, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightHand, 2*maxAngularVelocity);
}
UpdateTransform (ref leftHip, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftHip, maxAngularVelocity);
UpdateTransform (ref rightHip, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightHip, maxAngularVelocity);
UpdateTransform (ref leftKnee, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftKnee, maxAngularVelocity);
UpdateTransform (ref rightKnee, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightKnee, maxAngularVelocity);
UpdateTransform (ref rightElbow, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightElbow, maxAngularVelocity);
UpdateTransform (ref leftElbow, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftElbow, maxAngularVelocity);
if(trackAnkle || !useHierarchicalModel)
{
UpdateTransform (ref leftFoot, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftFoot, maxAngularVelocity);
UpdateTransform (ref rightFoot, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightFoot, maxAngularVelocity);
}
// // TODO: Restore this when implementation is fixed
// if(rotateWristFromElbow && bodyTrackingDevice == bodyTrackingDeviceType.Kinect2)
// {
// if (useHierarchicalModel)
// {
// if(leftElbow && leftHand)
// leftElbow.rotation = leftHand.rotation;
// if(rightElbow && rightHand)
// rightElbow.rotation = rightHand.rotation;
// }
// else
// {
// if(leftElbow && leftHand)
// leftElbow.localRotation = leftHand.localRotation;
// if(rightElbow && rightHand)
// rightElbow.localRotation = rightHand.localRotation;
// }
// // UpdateTransform (ref rightElbow, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightHand);
// // UpdateTransform (ref leftElbow, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftHand);
// }
if(bodyTrackingDevice == bodyTrackingDeviceType.Kinect2 || bodyTrackingDevice == bodyTrackingDeviceType.GenericMotionTracker)
{
if(fistCurlFingers)
handleFingersCurling(trackThumbs);
if(trackThumbs)
{
if(rightThumb)
UpdateTransform (ref rightThumb, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightThumb, maxAngularVelocity);
if(leftThumb)
UpdateTransform (ref leftThumb, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftThumb, maxAngularVelocity);
}
}
}
if (!useHierarchicalModel)
{
if (leftHand != null)
{
leftHand.localRotation = leftElbow.localRotation;
}
if (rightHand != null)
{
rightHand.localRotation = rightElbow.localRotation;
}
} else
{
if (scaleHierarchicalModelBones)
{
UpdateBoneScalings ();
torsoRotation = Quaternion.Slerp(torsoRotation, skeletonManager.skeletons[bodyTrackingDeviceID, playerId].torso.rotation, Time.deltaTime*rotationDamping);
torsoDirection = torsoRotation * Vector3.down;
if(torso == root)
torso.position = transform.TransformPoint (- torsoDirection * (torsoOffset * torsoScale + adjustVerticalHipsPosition));
else
torso.position = transform.TransformPoint (skeletonManager.skeletons [bodyTrackingDeviceID, playerId].torso.position - skeletonPosition
- torsoDirection * (torsoOffset * torsoScale + adjustVerticalHipsPosition));
spineDirection = transform.TransformPoint (skeletonManager.skeletons [bodyTrackingDeviceID, playerId].torso.position - skeletonPosition
- torsoDirection * (torsoOffset * torsoScale + adjustVerticalHipsPosition - 1));
spineDirection = torso.position - spineDirection;
spineDirection.Normalize();
// Obtained new body tracking data. TODO test that Kinect 1 still works
// if(bodyTrackingDeviceID != RUISSkeletonManager.kinect2SensorID || skeletonManager.isNewKinect2Frame)
{
float deltaT;
// if(bodyTrackingDeviceID == RUISSkeletonManager.kinect2SensorID)
// deltaT = skeletonManager.kinect2FrameDeltaT;
// else
deltaT = Time.deltaTime;
ForceUpdatePosition (ref rightShoulder, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightShoulder, 0, deltaT);
ForceUpdatePosition (ref leftShoulder, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftShoulder, 1, deltaT);
ForceUpdatePosition (ref rightHip, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].rightHip, 2, deltaT);
ForceUpdatePosition (ref leftHip, skeletonManager.skeletons [bodyTrackingDeviceID, playerId].leftHip, 3, deltaT);
}
}
}
if (updateRootPosition)
{
// Vector3 newRootPosition = skeletonManager.skeletons [bodyTrackingDeviceID, playerId].root.position;
// measuredPos [0] = newRootPosition.x;
// measuredPos [1] = newRootPosition.y;
// measuredPos [2] = newRootPosition.z;
// positionKalman.setR (Time.deltaTime * positionNoiseCovariance);
// positionKalman.predict ();
// positionKalman.update (measuredPos);
// pos = positionKalman.getState ();
// Root speed scaling is applied here
transform.localPosition = Vector3.Scale(skeletonPosition, rootSpeedScaling);
// transform.localPosition = Vector3.Scale(new Vector3 ((float)pos [0], (float)pos [1], (float)pos [2]), rootSpeedScaling);
}
}
if(characterController)
{
// If character controller pivot is PS Move
if (followMoveController && inputManager)
{
psmove = inputManager.GetMoveWand (followMoveID);
if (psmove)
{
float moveYaw = psmove.localRotation.eulerAngles.y;
trackedDeviceYawRotation = Quaternion.Euler (0, moveYaw, 0);
if(!skeletonManager.skeletons [bodyTrackingDeviceID, playerId].isTracking)
{
skeletonPosition = psmove.localPosition - trackedDeviceYawRotation * characterController.psmoveOffset;
skeletonPosition.y = 0;
if (updateRootPosition)
transform.localPosition = skeletonPosition;
if(characterController.headRotatesBody)
UpdateTransformWithTrackedDevice (ref root, moveYaw);
// UpdateTransformWithPSMove (ref torso, moveYaw);
// UpdateTransformWithPSMove (ref head, moveYawRotation);
// UpdateTransformWithPSMove (ref leftShoulder, moveYawRotation);
// UpdateTransformWithPSMove (ref leftElbow, moveYawRotation);
// UpdateTransformWithPSMove (ref leftHand, moveYawRotation);
// UpdateTransformWithPSMove (ref rightShoulder, moveYawRotation);
// UpdateTransformWithPSMove (ref rightElbow, moveYawRotation);
// UpdateTransformWithPSMove (ref rightHand, moveYawRotation);
// UpdateTransformWithPSMove (ref leftHip, moveYawRotation);
// UpdateTransformWithPSMove (ref leftKnee, moveYawRotation);
// UpdateTransformWithPSMove (ref leftFoot, moveYawRotation);
// UpdateTransformWithPSMove (ref rightHip, moveYawRotation);
// UpdateTransformWithPSMove (ref rightKnee, moveYawRotation);
// UpdateTransformWithPSMove (ref rightFoot, moveYawRotation);
}
}
}
if(followOculusController)
{
float oculusYaw = 0;
if(coordinateSystem)
{
if(coordinateSystem.applyToRootCoordinates)
{
if(ovrHmdVersion == Ovr.HmdType.DK1 || ovrHmdVersion == Ovr.HmdType.DKHD)
oculusYaw = coordinateSystem.GetOculusRiftOrientationRaw().eulerAngles.y;
else
{
skeletonPosition = coordinateSystem.ConvertLocation(coordinateSystem.GetOculusRiftLocation(), RUISDevice.Oculus_DK2);
skeletonPosition.y = 0;
oculusYaw = coordinateSystem.ConvertRotation(Quaternion.Inverse(coordinateSystem.GetOculusCameraOrientationRaw()) * coordinateSystem.GetOculusRiftOrientationRaw(),
RUISDevice.Oculus_DK2).eulerAngles.y;
}
}
else if(OVRManager.display != null)
{
skeletonPosition = OVRManager.display.GetHeadPose().position;
skeletonPosition.y = 0;
oculusYaw = OVRManager.display.GetHeadPose().orientation.eulerAngles.y;
}
}
trackedDeviceYawRotation = Quaternion.Euler (0, oculusYaw, 0);
if(updateRootPosition)
transform.localPosition = skeletonPosition;
if(characterController.headRotatesBody)
UpdateTransformWithTrackedDevice (ref root, oculusYaw);
}
}
TweakHipPosition();
TweakNeckHeight();
}
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;
}
public Vector3 TransformDirection(Vector3 directionInCharacterCoordinates)
{
Vector3 characterForward = Vector3.forward;
if (skeletonController)
{
characterForward = skeletonController.transform.localRotation * Vector3.forward;
}
switch (characterPivotType)
{
case CharacterPivotType.KinectHead:
if ((bodyTrackingDeviceID == RUISSkeletonManager.kinect2SensorID && !inputManager.enableKinect2) ||
(bodyTrackingDeviceID == RUISSkeletonManager.kinect1SensorID && !inputManager.enableKinect))
{
break;
}
if (skeletonManager != null && skeletonManager.skeletons[bodyTrackingDeviceID, kinectPlayerId] != null)
{
characterForward = skeletonManager.skeletons[bodyTrackingDeviceID, kinectPlayerId].head.rotation * Vector3.forward;
}
else
{
characterForward = Vector3.forward;
}
break;
case CharacterPivotType.KinectTorso:
if ((bodyTrackingDeviceID == RUISSkeletonManager.kinect2SensorID && !inputManager.enableKinect2) ||
(bodyTrackingDeviceID == RUISSkeletonManager.kinect1SensorID && !inputManager.enableKinect))
{
break;
}
if (skeletonManager != null && skeletonManager.skeletons[bodyTrackingDeviceID, kinectPlayerId] != null)
{
characterForward = skeletonManager.skeletons[bodyTrackingDeviceID, kinectPlayerId].torso.rotation * Vector3.forward;
}
else
{
characterForward = Vector3.forward;
}
break;
case CharacterPivotType.MoveController:
{
if (!inputManager.enablePSMove || !headPointsWalkingDirection)
{
break;
}
RUISPSMoveWand psmove = inputManager.GetMoveWand(moveControllerId);
if (psmove != null)
{
characterForward = psmove.localRotation * Vector3.forward;
}
}
break;
}
if (skeletonManager != null && (skeletonController.followOculusController || skeletonController.followMoveController) && headPointsWalkingDirection)
{
characterForward = skeletonController.trackedDeviceYawRotation * Vector3.forward;
}
if (ignorePitchAndRoll)
{
characterForward.y = 0;
characterForward.Normalize();
}
characterForward = transform.TransformDirection(characterForward);
return(Quaternion.LookRotation(characterForward, transform.up) * directionInCharacterCoordinates);
}
void LateUpdate()
{
if (skeletonManager != null && skeletonManager.skeletons[playerId] != null && skeletonManager.skeletons[playerId].isTracking)
{
UpdateSkeletonPosition();
UpdateTransform(ref head, skeletonManager.skeletons[playerId].head);
UpdateTransform(ref torso, skeletonManager.skeletons[playerId].torso);
UpdateTransform(ref leftShoulder, skeletonManager.skeletons[playerId].leftShoulder);
UpdateTransform(ref leftElbow, skeletonManager.skeletons[playerId].leftElbow);
UpdateTransform(ref leftHand, skeletonManager.skeletons[playerId].leftHand);
UpdateTransform(ref rightShoulder, skeletonManager.skeletons[playerId].rightShoulder);
UpdateTransform(ref rightElbow, skeletonManager.skeletons[playerId].rightElbow);
UpdateTransform(ref rightHand, skeletonManager.skeletons[playerId].rightHand);
//UpdateTransform(ref leftHip, skeletonManager.skeletons[playerId].leftHip);
leftHip.rotation = Quaternion.LookRotation(new Vector3(root.parent.forward.x, 0, root.parent.forward.z))*Quaternion.Euler(new Vector3(-70, 0, 0 ));
UpdateTransform(ref leftKnee, skeletonManager.skeletons[playerId].leftKnee);
UpdateTransform(ref leftFoot, skeletonManager.skeletons[playerId].leftFoot);
//UpdateTransform(ref rightHip, skeletonManager.skeletons[playerId].rightHip);
rightHip.rotation = Quaternion.LookRotation(new Vector3(root.parent.forward.x, 0, root.parent.forward.z))*Quaternion.Euler(new Vector3(-70, 0, 0 )); UpdateTransform(ref rightKnee, skeletonManager.skeletons[playerId].rightKnee);
UpdateTransform(ref rightFoot, skeletonManager.skeletons[playerId].rightFoot);
if (!useHierarchicalModel)
{
if (leftHand != null)
{
leftHand.localRotation = leftElbow.localRotation;
}
if (rightHand != null)
{
rightHand.localRotation = rightElbow.localRotation;
}
}
else
{
if (scaleHierarchicalModelBones)
{
UpdateBoneScalings();
Vector3 torsoDirection = skeletonManager.skeletons[playerId].torso.rotation * Vector3.down;
torso.position = transform.TransformPoint(skeletonManager.skeletons[playerId].torso.position - skeletonPosition - torsoDirection * torsoOffset * torsoScale);
ForceUpdatePosition(ref rightShoulder, skeletonManager.skeletons[playerId].rightShoulder);
ForceUpdatePosition(ref leftShoulder, skeletonManager.skeletons[playerId].leftShoulder);
ForceUpdatePosition(ref rightHip, skeletonManager.skeletons[playerId].rightHip);
ForceUpdatePosition(ref leftHip, skeletonManager.skeletons[playerId].leftHip);
}
}
if (updateRootPosition)
{
Vector3 newRootPosition = skeletonManager.skeletons[playerId].root.position;
measuredPos[0] = newRootPosition.x;
measuredPos[1] = newRootPosition.y;
measuredPos[2] = newRootPosition.z;
positionKalman.setR(Time.deltaTime * positionNoiseCovariance);
positionKalman.predict();
positionKalman.update(measuredPos);
pos = positionKalman.getState();
transform.localPosition = new Vector3((float) pos[0], (float) pos[1], (float) pos[2]); //newRootPosition;
}
}
else // TUUKKA
if(followMoveController && characterController && inputManager)
{
psmove = inputManager.GetMoveWand(followMoveID);
if(psmove)
{
Quaternion moveYaw = Quaternion.Euler(0, psmove.localRotation.eulerAngles.y, 0);
skeletonPosition = psmove.localPosition - moveYaw*characterController.psmoveOffset;
skeletonPosition.y = 0;
if (updateRootPosition)
transform.localPosition = skeletonPosition;
UpdateTransformWithPSMove(ref head, moveYaw);
UpdateTransformWithPSMove(ref torso, moveYaw);
UpdateTransformWithPSMove(ref leftShoulder, moveYaw);
UpdateTransformWithPSMove(ref leftElbow, moveYaw);
UpdateTransformWithPSMove(ref leftHand, moveYaw);
UpdateTransformWithPSMove(ref rightShoulder, moveYaw);
UpdateTransformWithPSMove(ref rightElbow, moveYaw);
UpdateTransformWithPSMove(ref rightHand, moveYaw);
UpdateTransformWithPSMove(ref leftHip, moveYaw);
UpdateTransformWithPSMove(ref leftKnee, moveYaw);
UpdateTransformWithPSMove(ref leftFoot, moveYaw);
UpdateTransformWithPSMove(ref rightHip, moveYaw);
UpdateTransformWithPSMove(ref rightKnee, moveYaw);
UpdateTransformWithPSMove(ref rightFoot, moveYaw);
}
}
TweakNeckHeight();
}
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;
}
void LateUpdate()
{
if (skeletonManager != null && skeletonManager.skeletons[playerId] != null && skeletonManager.skeletons[playerId].isTracking)
{
UpdateSkeletonPosition();
UpdateTransform(ref head, skeletonManager.skeletons[playerId].head);
UpdateTransform(ref torso, skeletonManager.skeletons[playerId].torso);
UpdateTransform(ref leftShoulder, skeletonManager.skeletons[playerId].leftShoulder);
UpdateTransform(ref leftElbow, skeletonManager.skeletons[playerId].leftElbow);
UpdateTransform(ref leftHand, skeletonManager.skeletons[playerId].leftHand);
UpdateTransform(ref rightShoulder, skeletonManager.skeletons[playerId].rightShoulder);
UpdateTransform(ref rightElbow, skeletonManager.skeletons[playerId].rightElbow);
UpdateTransform(ref rightHand, skeletonManager.skeletons[playerId].rightHand);
UpdateTransform(ref leftHip, skeletonManager.skeletons[playerId].leftHip);
UpdateTransform(ref leftKnee, skeletonManager.skeletons[playerId].leftKnee);
UpdateTransform(ref leftFoot, skeletonManager.skeletons[playerId].leftFoot);
UpdateTransform(ref rightHip, skeletonManager.skeletons[playerId].rightHip);
UpdateTransform(ref rightKnee, skeletonManager.skeletons[playerId].rightKnee);
UpdateTransform(ref rightFoot, skeletonManager.skeletons[playerId].rightFoot);
if (!useHierarchicalModel)
{
if (leftHand != null)
{
leftHand.localRotation = leftElbow.localRotation;
}
if (rightHand != null)
{
rightHand.localRotation = rightElbow.localRotation;
}
}
else
{
if (scaleHierarchicalModelBones)
{
UpdateBoneScalings();
Vector3 torsoDirection = skeletonManager.skeletons[playerId].torso.rotation * Vector3.down;
torso.position = transform.TransformPoint(skeletonManager.skeletons[playerId].torso.position - skeletonPosition - torsoDirection * torsoOffset * torsoScale);
ForceUpdatePosition(ref rightShoulder, skeletonManager.skeletons[playerId].rightShoulder);
ForceUpdatePosition(ref leftShoulder, skeletonManager.skeletons[playerId].leftShoulder);
ForceUpdatePosition(ref rightHip, skeletonManager.skeletons[playerId].rightHip);
ForceUpdatePosition(ref leftHip, skeletonManager.skeletons[playerId].leftHip);
}
}
if (updateRootPosition)
{
Vector3 newRootPosition = skeletonManager.skeletons[playerId].root.position;
measuredPos[0] = newRootPosition.x;
measuredPos[1] = newRootPosition.y;
measuredPos[2] = newRootPosition.z;
positionKalman.setR(Time.deltaTime * positionNoiseCovariance);
positionKalman.predict();
positionKalman.update(measuredPos);
pos = positionKalman.getState();
transform.localPosition = new Vector3((float)pos[0], (float)pos[1], (float)pos[2]); //newRootPosition;
}
}
else // TUUKKA
if (followMoveController && characterController && inputManager)
{
psmove = inputManager.GetMoveWand(followMoveID);
if (psmove)
{
Quaternion moveYaw = Quaternion.Euler(0, psmove.localRotation.eulerAngles.y, 0);
skeletonPosition = psmove.localPosition - moveYaw * characterController.psmoveOffset;
skeletonPosition.y = 0;
if (updateRootPosition)
{
transform.localPosition = skeletonPosition;
}
UpdateTransformWithPSMove(ref head, moveYaw);
UpdateTransformWithPSMove(ref torso, moveYaw);
UpdateTransformWithPSMove(ref leftShoulder, moveYaw);
UpdateTransformWithPSMove(ref leftElbow, moveYaw);
UpdateTransformWithPSMove(ref leftHand, moveYaw);
UpdateTransformWithPSMove(ref rightShoulder, moveYaw);
UpdateTransformWithPSMove(ref rightElbow, moveYaw);
UpdateTransformWithPSMove(ref rightHand, moveYaw);
UpdateTransformWithPSMove(ref leftHip, moveYaw);
UpdateTransformWithPSMove(ref leftKnee, moveYaw);
UpdateTransformWithPSMove(ref leftFoot, moveYaw);
UpdateTransformWithPSMove(ref rightHip, moveYaw);
UpdateTransformWithPSMove(ref rightKnee, moveYaw);
UpdateTransformWithPSMove(ref rightFoot, moveYaw);
}
}
TweakNeckHeight();
}
void Awake()
{
psMoveController = GetComponent<RUISPSMoveWand>();
wandSelector = GetComponent<RUISWandSelector>();
}
void Awake()
{
moveController = GetComponent<RUISPSMoveWand>();
}
void Awake()
{
psMoveController = GetComponent <RUISPSMoveWand>();
wandSelector = GetComponent <RUISWandSelector>();
}
void Awake()
{
moveController = GetComponent <RUISPSMoveWand>();
}
