//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);
}
/*
* Create the chaparone quads.
*
* @return Success
*/
private bool CreateChaporoneQuads()
{
if (_chaperoneQuads == null)
{
CVRChaperoneSetup chaperone = OpenVR.ChaperoneSetup;
bool success = (chaperone != null) && chaperone.GetLiveCollisionBoundsInfo(out _chaperoneQuads);
if (!success)
{
Debug.LogError("Failed to get Calibrated Chaperone bounds! Make sure you have tracking first, and that your space is calibrated.");
return(false);
}
}
return(true);
}
//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();
}
}
// Use this for initialization
void Start()
{
var error = EVRInitError.None;
var pChaperone = OpenVR.GetGenericInterface(OpenVR.IVRChaperone_Version, ref error);
if (pChaperone == System.IntPtr.Zero || error != EVRInitError.None)
{
// if (!SteamVR.active)
// OpenVR.Shutdown();
return;
}
var chaperone = new CVRChaperone(pChaperone);
var chaperoneSetup = new CVRChaperoneSetup(pChaperone);
//var pQuadsBuffer = new HmdQuad_t[];
HmdQuad_t[] pQuadsBuffer = null;
// //pQuadsBuffer= new HmdQuad_t[punQuadsCount];
//
// int punQuadsCount = 10;
// HmdQuad_t[] pqd = new HmdQuad_t[[punQuadsCount];
bool hasBounds = chaperoneSetup.GetLiveCollisionBoundsInfo(out pQuadsBuffer);
if (hasBounds == true)
{
print("YAYA");
print(pQuadsBuffer);
}
else
{
print("NOOO");
}
//print( chaperone );
//print( chaperoneSetup );
// print( rect );
}
//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.");
}
}
}
}
}
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);
}
// Use this for initialization
void Awake()
{
chaperoneSetup = OpenVR.ChaperoneSetup;
chaperone = OpenVR.Chaperone;
}
