//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;
}
//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");
}
//Actual calibration procedures
void Recalibrate(TrackedDevicePose_t[] poses, bool calibrate, bool useController, ref HmdMatrix34_t referenceMatrix)
{
if (calibrate)
{
if (measurementCount == 0)
{
if (useController)
{
TrackedDevicePose_t leftPose = poses [leftId];
TrackedDevicePose_t rightPose = poses [rightId];
//m10 = y-position
if (leftPose.mDeviceToAbsoluteTracking.m7 < rightPose.mDeviceToAbsoluteTracking.m7)
{
refControllerId = leftId;
}
else
{
refControllerId = rightId;
}
HmdMatrix34_t matrix = poses [refControllerId].mDeviceToAbsoluteTracking;
// Debug.Log (matrix.m3 + "," + matrix.m7 + "," + matrix.m11);
initialOffset = matrix.m7;
// Debug.Log (initialOffset);
tempOffsetY = matrix.m7;
tempRoll = Mathf.Atan2(matrix.m4, matrix.m5);
measurementCount = 1;
}
else
{
initialOffset = trackerMatrix.m7;
tempOffsetY = trackerMatrix.m7;
measurementCount = 1;
}
}
else
{
if (useController)
{
measurementCount++;
HmdMatrix34_t matrix = poses [refControllerId].mDeviceToAbsoluteTracking;
float rollDiff = Mathf.Atan2(matrix.m4, matrix.m5) - tempRoll;
if (rollDiff > Mathf.PI)
{
rollDiff -= 2.0f * Mathf.PI;
}
else if (rollDiff < -Mathf.PI)
{
rollDiff += 2.0f * Mathf.PI;
}
tempRoll += rollDiff / (float)measurementCount;
if (tempRoll > Mathf.PI)
{
tempRoll -= 2.0f * Mathf.PI;
}
else
if (tempRoll < -Mathf.PI)
{
tempRoll += 2.0f * Mathf.PI;
}
if (measurementCount >= 25)
{
if (Mathf.Abs(tempRoll) <= Mathf.PI / 2f)
{
floorOffset = tempOffsetY - controllerUpOffsetCorrection;
}
else
{
floorOffset = tempOffsetY - controllerDownOffsetCorrection;
}
AddOffsetToUniverseCenter(floorOffset, ref referenceMatrix);
// Debug.Log ("Fix Floor: Floor Offset = " + floorOffset);
Debug.Log("Registered center matrix : " + "(" + centerMatrix.m3 + ":" + centerMatrix.m7 + ":" + centerMatrix.m11 + ")");
firstCalibration = true;
}
}
//Using tracker for floor calibration
else
{
measurementCount++;
initialOffset = trackerMatrix.m7;
tempOffsetY = trackerMatrix.m7;
if (measurementCount >= 25)
{
// tempOffsetY -= 0.01f;
//Set current chaperone setting same as the server, then adjust coordinate by offset.
//NOT FEASIBLE [Different universe with diffcenter coordinate]
CVRChaperoneSetup setup = OpenVR.ChaperoneSetup;
setup.RevertWorkingCopy();
setup.SetWorkingStandingZeroPoseToRawTrackingPose(ref centerCoordinate);
setup.CommitWorkingCopy(EChaperoneConfigFile.Live);
referenceMatrix = centerCoordinate;
//
AddOffsetToUniverseCenter(tempOffsetY, ref referenceMatrix);
Debug.Log("Registered center matrix : " + "(" + centerMatrix.m3 + ":" + centerMatrix.m7 + ":" + centerMatrix.m11 + ") by tracker.");
}
}
}
}
}
