//Manually reset floor matrix
public void FixFloor(HmdMatrix34_t matrix)
{
Debug.Log("Calibrating floor....");
CVRChaperoneSetup setup = OpenVR.ChaperoneSetup;
double[] dataObtained = GetUniverseOffset();
HmdMatrix34_t currentMatrix = new HmdMatrix34_t();
setup.RevertWorkingCopy();
setup.GetWorkingStandingZeroPoseToRawTrackingPose(ref currentMatrix);
matrix = currentMatrix;
double m1Offset = dataObtained[0] - universePitchRollVariance [0];
double m9Offset = dataObtained[1] - universePitchRollVariance [1];
double variance = dataObtained[2] - universePitchRollVariance [2];
Debug.Log(dataObtained[0] + ":" + universePitchRollVariance[0]);
float m1 = (float)m1Offset;
float m9 = (float)m9Offset;
// float resultant = Mathf.Sqrt ((m1 * m1) + (m9 * m9));
float resultant = Mathf.Min(Mathf.Abs(m1), Mathf.Abs(m9));
currentMatrix.m7 -= currentMatrix.m5 * resultant;
Debug.Log("m1Offset = " + m1Offset + " : m9Offset = " + m9Offset);
setup.RevertWorkingCopy();
setup.SetWorkingStandingZeroPoseToRawTrackingPose(ref currentMatrix);
setup.CommitWorkingCopy(EChaperoneConfigFile.Live);
FloorFixed.Trigger();
Debug.Log("Floor calibrated from (" + matrix.m3 + "," + matrix.m7 + "," + matrix.m11 + ") \nto (" + currentMatrix.m3 + "," + currentMatrix.m7 + "," + currentMatrix.m11 + ")");
timer = 6f;
universePitchRollVariance = dataObtained;
}
public float GetHeight()
{
CVRChaperoneSetup setup = OpenVR.ChaperoneSetup;
HmdMatrix34_t currentMatrix = new HmdMatrix34_t();
setup.RevertWorkingCopy();
setup.GetWorkingStandingZeroPoseToRawTrackingPose(ref currentMatrix);
return(currentMatrix.m7);
}
//Manually set floor height
public static void FixFloor(float height)
{
CVRChaperoneSetup setup = OpenVR.ChaperoneSetup;
HmdMatrix34_t currentMatrix = new HmdMatrix34_t();
setup.RevertWorkingCopy();
setup.GetWorkingStandingZeroPoseToRawTrackingPose(ref currentMatrix);
currentMatrix.m7 = height;
setup.SetWorkingStandingZeroPoseToRawTrackingPose(ref currentMatrix);
setup.CommitWorkingCopy(EChaperoneConfigFile.Live);
}
//Manually set floor height
public void FixFloor(float height)
{
Debug.Log("Calibrating floor....Setting height at " + height + " m");
CVRChaperoneSetup setup = OpenVR.ChaperoneSetup;
HmdMatrix34_t currentMatrix = new HmdMatrix34_t();
setup.RevertWorkingCopy();
setup.GetWorkingStandingZeroPoseToRawTrackingPose(ref currentMatrix);
currentMatrix.m7 = height;
setup.SetWorkingStandingZeroPoseToRawTrackingPose(ref currentMatrix);
setup.CommitWorkingCopy(EChaperoneConfigFile.Live);
FloorFixed.Trigger();
Debug.Log("Floor calibrated at height " + height + " m");
hmdResumeTracking = false;
}
//Automatically set floor height based on controller coordinates
void AddOffsetToUniverseCenter(float offset)
{
if (offset != 0f)
{
HmdMatrix34_t currentMatrix = new HmdMatrix34_t();
CVRChaperoneSetup setup = OpenVR.ChaperoneSetup;
setup.RevertWorkingCopy();
setup.GetWorkingStandingZeroPoseToRawTrackingPose(ref currentMatrix);
currentMatrix.m3 += currentMatrix.m1 * offset;
currentMatrix.m7 += currentMatrix.m5 * offset;
currentMatrix.m11 += currentMatrix.m9 * offset;
setup.SetWorkingStandingZeroPoseToRawTrackingPose(ref currentMatrix);
setup.CommitWorkingCopy(EChaperoneConfigFile.Live);
ResetAttributes();
}
}
//Automatically set floor height based on controller coordinatess
void AddOffsetToUniverseCenter(float offset, ref HmdMatrix34_t referenceMatrix)
{
if (offset != 0f)
{
HmdMatrix34_t currentMatrix = new HmdMatrix34_t();
CVRChaperoneSetup setup = OpenVR.ChaperoneSetup;
setup.RevertWorkingCopy();
setup.GetWorkingStandingZeroPoseToRawTrackingPose(ref currentMatrix);
currentMatrix.m3 += currentMatrix.m1 * offset;
currentMatrix.m7 += currentMatrix.m5 * offset;
currentMatrix.m11 += currentMatrix.m9 * offset;
setup.SetWorkingStandingZeroPoseToRawTrackingPose(ref currentMatrix);
referenceMatrix = currentMatrix;
setup.CommitWorkingCopy(EChaperoneConfigFile.Live);
universePitchRollVariance = GetUniverseOffset();
ResetAttributes();
FloorFixed.Trigger();
}
}
public void FixFloor(float height, string direction)
{
Debug.Log("Calibrating floor....Setting height at " + height + " m");
CVRChaperoneSetup setup = OpenVR.ChaperoneSetup;
HmdMatrix34_t currentMatrix = new HmdMatrix34_t();
setup.RevertWorkingCopy();
setup.GetWorkingStandingZeroPoseToRawTrackingPose(ref currentMatrix);
if (direction == "UP")
{
currentMatrix.m7 -= currentMatrix.m5 * height;
}
else if (direction == "DOWN")
{
currentMatrix.m7 += currentMatrix.m5 * height;
}
else if (direction == "LEFT")
{
currentMatrix.m3 -= height;
}
else if (direction == "RIGHT")
{
currentMatrix.m3 += height;
}
else if (direction == "FORWARD")
{
currentMatrix.m11 += height;
}
else if (direction == "BACKWARD")
{
currentMatrix.m11 -= height;
}
setup.SetWorkingStandingZeroPoseToRawTrackingPose(ref currentMatrix);
setup.CommitWorkingCopy(EChaperoneConfigFile.Live);
FloorFixed.Trigger();
Debug.Log("Floor calibrated at height " + height + " m");
}
void OnNewPoses(TrackedDevicePose_t[] poses)
{
if (!poses [0].bPoseIsValid)
{
hmdLostTrack = true;
hmdResumeTracking = false;
HmdLostTrack.Invoke();
}
else
{
if (hmdLostTrack)
{
hmdResumeTracking = true;
hmdLostTrack = false;
}
}
if (autoCalibration)
{
// if (!firstCalibration)
// {
// Recalibrate (poses, canAdjust, ref centerMatrix);
// }
// else
// {
// if (canCalibrateStandingPose)
// {
// RegisterOffsetToFloor (poses);
// Debug.Log ("Offset to floor registered");
// standingPoseCalibrated = true;
// canCalibrateStandingPose = false;
// }
//
if (canCalibrateByTracker)
{
// FixFloor (centerMatrix);
// RemoveErrorOffset(poses);
Recalibrate(poses, true, true, ref centerMatrix);
}
//
// if (canCalibrateByController)
// {
// Recalibrate (poses, true, true, ref centerMatrix);
// }
// }
}
if (!controllerRegistered)
{
leftId = SteamVR_Controller.GetDeviceIndex(SteamVR_Controller.DeviceRelation.Leftmost);
rightId = SteamVR_Controller.GetDeviceIndex(SteamVR_Controller.DeviceRelation.Rightmost);
trackerId = SteamVR_Controller.GetDeviceIndex(SteamVR_Controller.DeviceRelation.First, ETrackedDeviceClass.GenericTracker);
}
if (leftId == rightId)
{
Debug.Log("Right controller not detected");
}
else
{
controllerRegistered = true;
device = SteamVR_Controller.Input(rightId);
}
if (controllerRegistered)
{
TrackedDevicePose_t leftPose = poses [leftId];
TrackedDevicePose_t rightPose = poses[rightId];
leftMatrix = leftPose.mDeviceToAbsoluteTracking;
rightMatrix = leftPose.mDeviceToAbsoluteTracking;
if (leftPose.mDeviceToAbsoluteTracking.m7 < -0.01f)
{
Debug.Log("Left controller calibration error");
controllerCalibrationError = true;
}
else if (rightPose.mDeviceToAbsoluteTracking.m7 < -0.01f)
{
Debug.Log("Right controller calibration error");
controllerCalibrationError = true;
}
if (trackerId > 0)
{
TrackedDevicePose_t trackerPose = poses [trackerId];
trackerMatrix = trackerPose.mDeviceToAbsoluteTracking;
gameManager.SyncTrackerPosition(trackerMatrix);
}
else
{
trackerMatrix = gameManager.GetTrackerPosition();
centerCoordinate = gameManager.GetChaperoneCenterCoordinate();
}
if (gameManager.isServer)
{
CVRChaperoneSetup setup = OpenVR.ChaperoneSetup;
setup.RevertWorkingCopy();
setup.GetWorkingStandingZeroPoseToRawTrackingPose(ref centerMatrix);
gameManager.SyncChaperoneCenterCoordinate(centerMatrix);
centerCoordinate = centerMatrix;
}
}
// //Get & display chaperone center point coordinates
// CVRChaperoneSetup setup = OpenVR.ChaperoneSetup;
// setup.GetWorkingStandingZeroPoseToRawTrackingPose (ref centerMatrix);
// Debug.Log ("Center Coordinate : " + centerMatrix.m3 + "," + centerMatrix.m7 + "," + centerMatrix.m11);
}
