public bool GetDeviceTransformByIndex_SteamVR(int index, out Vector3 position, out Quaternion rotation)
{
if (index < 0 || index >= trackedObjects.Length)
{
position = Vector3.zero;
rotation = Quaternion.identity;
return(false);
}
TrackedDevicePose_t pose = trackedObjects[index];
if (pose.bDeviceIsConnected && pose.bPoseIsValid && pose.eTrackingResult == ETrackingResult.Running_OK)
{
Matrix4x4 poseMat = TrackingSpaceOrigin.ConvertMatrixOpenVRToUnity(pose.mDeviceToAbsoluteTracking);
position = TrackingSpaceOrigin.ExtractPosition(poseMat);
position.z *= -1; //flip for OpenGL to DirectX conversion
Quaternion quat = TrackingSpaceOrigin.ExtractRotation(poseMat);
quat = Quaternion.Euler(new Vector3(-quat.eulerAngles.x, -quat.eulerAngles.y, quat.eulerAngles.z));
rotation = quat;
return(true);
}
else
{
position = Vector3.zero;
rotation = Quaternion.identity;
return(false);
}
}
/// <summary>
/// Gets the current controller state.
/// </summary>
/// <param name="left">Left or right hand controller.</param>
/// <returns>The controller state.</returns>
public VRController GetController(bool left)
{
VRControllerState_t vrcont = new VRControllerState_t();
TrackedDevicePose_t vrpose = new TrackedDevicePose_t();
bool valid = VR.GetControllerStateWithPose(ETrackingUniverseOrigin.TrackingUniverseStanding, VR.GetTrackedDeviceIndexForControllerRole(left ? ETrackedControllerRole.LeftHand : ETrackedControllerRole.RightHand), ref vrcont, ref vrpose);
if (!valid || !vrpose.bPoseIsValid)
{
return(null);
}
HmdMatrix34_t tmat = vrpose.mDeviceToAbsoluteTracking;
Matrix4 resp = new Matrix4(tmat.m0, tmat.m1, tmat.m2, tmat.m3, tmat.m4, tmat.m5, tmat.m6, tmat.m7, tmat.m8, tmat.m9, tmat.m10, tmat.m11, 0, 0, 0, 1);
resp.Transpose();
resp = resp.ClearTranslation() * Matrix4.CreateTranslation(resp.ExtractTranslation() * VRScale);
resp = resp * Matrix4.CreateRotationX((float)(Math.PI * 0.5));
VRController res = new VRController()
{
Position = resp,
Touched = (VRButtons)vrcont.ulButtonTouched,
Pressed = (VRButtons)vrcont.ulButtonPressed
};
res.Axes[0] = new Vector2(vrcont.rAxis0.x, vrcont.rAxis0.y);
res.Axes[1] = new Vector2(vrcont.rAxis1.x, vrcont.rAxis1.y);
res.Axes[2] = new Vector2(vrcont.rAxis2.x, vrcont.rAxis2.y);
res.Axes[3] = new Vector2(vrcont.rAxis3.x, vrcont.rAxis3.y);
res.Axes[4] = new Vector2(vrcont.rAxis4.x, vrcont.rAxis4.y);
return(res);
}
private void MaybeStartTracking(FrameUpdateParameters updateParameters, RigidBoneSystemInputs inputs, ControlVertexInfo[] previousFrameControlVertexInfos)
{
if (tracking == true)
{
//already tracking
return;
}
if (!stateTracker.NonMenuActive)
{
return;
}
bool triggerPressed = stateTracker.IsPressed(EVRButtonId.k_EButton_SteamVR_Trigger);
if (!triggerPressed)
{
return;
}
tracking = true;
TrackedDevicePose_t gamePose = updateParameters.GamePoses[stateTracker.DeviceIdx];
Matrix controllerTransform = gamePose.mDeviceToAbsoluteTracking.Convert();
DualQuaternion controllerTransformDq = DualQuaternion.FromMatrix(controllerTransform);
var worldSourcePosition = controllerTransformDq.Translation * 100;
var worldSourceOrientation = controllerTransformDq.Rotation;
sourceBone = parentInstance.MapPositionToBone(worldSourcePosition, previousFrameControlVertexInfos);
var inverseSourceBoneTotalTransform = sourceBone.GetChainedTransform(inputs).Invert();
boneRelativeSourcePosition = inverseSourceBoneTotalTransform.Transform(worldSourcePosition) - sourceBone.CenterPoint;
boneRelativeSourceOrientation = worldSourceOrientation.Chain(inverseSourceBoneTotalTransform.Rotation);
}
private void addPoseTrackerDistance(int trackerNum)
{
for (int po_i = 0; po_i < latestPoses.Length; po_i++)
{
TrackedDevicePose_t trackedDevicePose = latestPoses[po_i];
SteamVR_TrackedObject.EIndex eIndex = (SteamVR_TrackedObject.EIndex)po_i;
if (trackedDevicePose.bDeviceIsConnected &&
trackedDevicePose.bPoseIsValid &&
eIndex != SteamVR_TrackedObject.EIndex.Hmd)
{
var pose = new SteamVR_Utils.RigidTransform(trackedDevicePose.mDeviceToAbsoluteTracking);
Vector3 trackedObjectOrigin = trackedObjects[trackerNum].gameObject.transform.position;
float dist = Vector3.Distance(pose.pos, trackedObjectOrigin);
if (!distances[trackerNum].ContainsKey(eIndex))
{
distances[trackerNum].Add(eIndex, new List <float>()
{
dist
});
}
else
{
distances[trackerNum][eIndex].Add(dist);
}
}
}
}
private InverseKinematicsGoal MaybeContinueTracking(FrameUpdateParameters updateParameters)
{
if (!tracking)
{
return(null);
}
if (!stateTracker.NonMenuActive)
{
tracking = false;
return(null);
}
bool triggerPressed = stateTracker.IsPressed(EVRButtonId.k_EButton_SteamVR_Trigger);
if (!triggerPressed)
{
tracking = false;
return(null);
}
TrackedDevicePose_t gamePose = updateParameters.GamePoses[stateTracker.DeviceIdx];
Matrix controllerTransform = gamePose.mDeviceToAbsoluteTracking.Convert();
var controllerTransformDq = DualQuaternion.FromMatrix(controllerTransform);
var targetPosition = controllerTransformDq.Translation * 100;
var targetOrientation = controllerTransformDq.Rotation;
return(new InverseKinematicsGoal(sourceBone,
boneRelativeSourcePosition, boneRelativeSourceOrientation,
targetPosition, targetOrientation));
}
public override HmdPoseState WaitForPoses()
{
EVRCompositorError compositorError = _compositor.WaitGetPoses(_devicePoses, new TrackedDevicePose_t[0]);
TrackedDevicePose_t hmdPose = _devicePoses[OVR.k_unTrackedDeviceIndex_Hmd];
Matrix deviceToAbsolute = ToSysMatrix(hmdPose.mDeviceToAbsoluteTracking);
deviceToAbsolute.Decompose(out _, out Quaternion deviceRotation, out Vector3 devicePosition);
Matrix.Invert(ref deviceToAbsolute, out Matrix absoluteToDevice);
Matrix viewLeft = absoluteToDevice * _headToEyeLeft;
Matrix viewRight = absoluteToDevice * _headToEyeRight;
Matrix.Invert(ref viewLeft, out Matrix invViewLeft);
invViewLeft.Decompose(out Vector3 scale, out Quaternion leftRotation, out Vector3 leftPosition);
Matrix.Invert(ref viewRight, out Matrix invViewRight);
invViewRight.Decompose(out _, out Quaternion rightRotation, out Vector3 rightPosition);
// HMD (meters) vs Flax (centimeters) scaling
leftPosition *= 100;
rightPosition *= 100;
// TODO: Expose mDeviceToAbsoluteTracking as "Head position/rotation" so we can move master actor properly
return(new HmdPoseState(
devicePosition, deviceRotation,
_projLeft, _projRight,
leftPosition, rightPosition,
leftRotation, rightRotation));
}
/// <summary>
/// Track the devices for SteamVR and applying a delay.
/// <summary>
protected void OnNewPoses(TrackedDevicePose_t newpose)
{
if (index == EIndex.None)
{
return;
}
var i = (int)index;
isValid = false;
if (!newpose.bDeviceIsConnected)
{
return;
}
if (!newpose.bPoseIsValid)
{
return;
}
isValid = true;
//Get the position and rotation of our tracked device.
var pose = new SteamVR_Utils.RigidTransform(newpose.mDeviceToAbsoluteTracking);
//Saving those values.
RegisterPosition(1, pose.pos, pose.rot);
//Delay the saved values inside GetValuePosition() by a factor of latencyCompensation in milliseconds.
sl.Pose p = GetValuePosition(1, (float)(latencyCompensation / 1000.0f));
transform.localPosition = p.translation;
transform.localRotation = p.rotation;
}
public bool update()
{
var error = EVRCompositorError.None;
error = OpenVR.Compositor.WaitGetPoses(renderPoseArray, gamePoseArray);
if (error != EVRCompositorError.None)
{
Warn.print("Error in WaitGetPoses: {0}", error);
return(false);
}
//get head position
TrackedDevicePose_t pose = renderPoseArray[OpenVR.k_unTrackedDeviceIndex_Hmd];
if (pose.bDeviceIsConnected == true && pose.bPoseIsValid == true)
{
myHeadPose = VR.convertToMatrix4(pose.mDeviceToAbsoluteTracking);
}
else
{
Warn.print("Error getting pose information for the HMD");
return(false);
}
//update camera matrix for each eye
for (int i = 0; i < 2; i++)
{
//TODO: this doesn't look right, but seems to work. Work through the math.
Matrix4 view = Matrix4.CreateTranslation(-position) * Matrix4.CreateFromQuaternion(orientation) * myEyeTransform[i].Inverted() * myHeadPose.Inverted();
myCameras[i].setView(view);
}
return(true);
}
void TrackerCoords()
{
TrackedDevicePose_t trackedDevicePose = new TrackedDevicePose_t();
VRControllerState_t controllerState = new VRControllerState_t();
for (int i = 0; i < trackers.Length; i++)
{
TrackerData pT = trackers[i];
if (pT.deviceId < 0 ||
!vrContext.IsTrackedDeviceConnected((uint)pT.deviceId))
{
continue;
}
unsafe
{
vrContext.GetControllerStateWithPose(ETrackingUniverseOrigin.TrackingUniverseStanding, (uint)pT.deviceId, ref controllerState, vrControllerStateSize, ref trackedDevicePose);
}
var mat = ConvertMatrix(trackedDevicePose.mDeviceToAbsoluteTracking);
pT.position = GetPosition(mat);
pT.rotation = GetRotation(mat);
pT.isValid = trackedDevicePose.bPoseIsValid;
}
}
public void Update(TrackedDevicePose_t poseData)
{
rotationMatrix[0] = poseData.mDeviceToAbsoluteTracking.m0;
rotationMatrix[1] = poseData.mDeviceToAbsoluteTracking.m1;
rotationMatrix[2] = poseData.mDeviceToAbsoluteTracking.m2;
rotationMatrix[3] = poseData.mDeviceToAbsoluteTracking.m4;
rotationMatrix[4] = poseData.mDeviceToAbsoluteTracking.m5;
rotationMatrix[5] = poseData.mDeviceToAbsoluteTracking.m6;
rotationMatrix[6] = poseData.mDeviceToAbsoluteTracking.m8;
rotationMatrix[7] = poseData.mDeviceToAbsoluteTracking.m9;
rotationMatrix[8] = poseData.mDeviceToAbsoluteTracking.m10;
var orientation = EasyOpenVRSingleton.Utils.RotationMatrixToYPR(poseData.mDeviceToAbsoluteTracking);
this.orientation.x = orientation.pitch;
this.orientation.y = orientation.yaw;
this.orientation.z = orientation.roll;
position.x = poseData.mDeviceToAbsoluteTracking.m3;
position.y = poseData.mDeviceToAbsoluteTracking.m7;
position.z = poseData.mDeviceToAbsoluteTracking.m11;
velocity.x = poseData.vVelocity.v0;
velocity.y = poseData.vVelocity.v1;
velocity.z = poseData.vVelocity.v2;
angularVelocity.x = poseData.vAngularVelocity.v0;
angularVelocity.y = poseData.vAngularVelocity.v1;
angularVelocity.z = poseData.vAngularVelocity.v2;
isConnected = poseData.bDeviceIsConnected;
isTracking = poseData.bPoseIsValid;
}
public virtual void OnNewPoses(TrackedDevicePose_t[] poses)
{
m_UseOnNewPoses = true;
if (unObjectId == -1)
{
return;
}
var i = (int)unObjectId;
isValid = false;
if (poses.Length <= i)
{
return;
}
if (!poses[i].bDeviceIsConnected)
{
return;
}
if (!poses[i].bPoseIsValid)
{
return;
}
isValid = true;
m_Pose = poses[i];
}
public static void RescanAllPositions()
{
var vrMatrix = new HmdMatrix34_t();
TrackedDevicePose_t[] pTrackedDevicePoseArray = new TrackedDevicePose_t[OpenVR.k_unMaxTrackedDeviceCount];
for (uint i = 0; i < OpenVR.k_unMaxTrackedDeviceCount; i++)
{
OpenVR.System.GetDeviceToAbsoluteTrackingPose(eOrg, fPredictedSecondsToPhotonsFromNow, pTrackedDevicePoseArray);
if (pTrackedDevicePoseArray[i].bPoseIsValid)
{
vrMatrix = pTrackedDevicePoseArray[i].mDeviceToAbsoluteTracking;
trackedPositions[i] = OpenVRMatrixToOpenTKMatrix(vrMatrix);
for (uint j = 0; j < maxPositionsBuffer; j++)
{
allPositions[j, i] = trackedPositions[i];
}
activePositions[i] = true;
activePositions[i] = true;
}
else
{
activePositions[i] = false;
}
}
}
//指定デバイスの姿勢情報を取得
public Transform GetTransform(uint index)
{
//有効なデバイスか
if (!IsDeviceValid(index))
{
return(null);
}
TrackedDevicePose_t Pose = allDevicePose[index];
SteamVR_Utils.RigidTransform trans = new SteamVR_Utils.RigidTransform(Pose.mDeviceToAbsoluteTracking);
Transform res = new Transform();
res.deviceid = index;
//右手系・左手系の変換をした
res.velocity[0] = Pose.vVelocity.v0;
res.velocity[1] = Pose.vVelocity.v1;
res.velocity[2] = -Pose.vVelocity.v2;
res.angularVelocity[0] = -Pose.vAngularVelocity.v0;
res.angularVelocity[1] = -Pose.vAngularVelocity.v1;
res.angularVelocity[2] = Pose.vAngularVelocity.v2;
res.position = trans.pos;
res.rotation = trans.rot;
return(res);
}
public static void ScanPositions()
{
var vrMatrix = new HmdMatrix34_t();
TrackedDevicePose_t[] pTrackedDevicePoseArray = new TrackedDevicePose_t[OpenVR.k_unMaxTrackedDeviceCount];
for (int j = maxPositionsBuffer - 1; j > 0; j--)
{
for (int i = 0; i < OpenVR.k_unMaxTrackedDeviceCount; i++)
{
allPositions[j, i] = allPositions[j - 1, i];
}
}
for (uint i = 0; i < OpenVR.k_unMaxTrackedDeviceCount; i++)
{
if (activePositions[i] == true)
{
OpenVR.System.GetDeviceToAbsoluteTrackingPose(eOrg, fPredictedSecondsToPhotonsFromNow, pTrackedDevicePoseArray);
if (pTrackedDevicePoseArray[i].bPoseIsValid)
{
vrMatrix = pTrackedDevicePoseArray[i].mDeviceToAbsoluteTracking;
allPositions[0, i] = OpenVRMatrixToOpenTKMatrix(vrMatrix);
}
}
}
for (uint i = 0; i < OpenVR.k_unMaxTrackedDeviceCount; i++)
{
trackedPositions[i] = allPositions[maxPositionsBuffer - 1, i];
}
}
void UpdateTrackedObj()
{
TrackedDevicePose_t[] allPoses = new TrackedDevicePose_t[OpenVR.k_unMaxTrackedDeviceCount];
_vrSystem.GetDeviceToAbsoluteTrackingPose(ETrackingUniverseOrigin.TrackingUniverseStanding, 0, allPoses);
for (int i = 0; i < _validDeviceIds.Count; i++)
{
if (i < targetObjs.Length)
{
var pose = allPoses[_validDeviceIds[i]];
var absTracking = pose.mDeviceToAbsoluteTracking;
var mat = new SteamVR_Utils.RigidTransform(absTracking);
targetObjs[i].transform.SetPositionAndRotation(mat.pos, mat.rot);
//Debug.Log(mat.pos+":"+ mat.rot);
float test;
this.GetComponent <InputVive> ().Vive_Y_set(mat.rot.y);
// Debug.Log (GetComponent<InputVive> ().Vive_Y_get());
}
}
}
public Dictionary <ETrackedDeviceClass, List <KeyValuePair <SteamVR_Utils.RigidTransform, string> > > GetTrackerPositions()
{
var positions = new Dictionary <ETrackedDeviceClass, List <KeyValuePair <SteamVR_Utils.RigidTransform, string> > > ();
positions.Add(ETrackedDeviceClass.HMD, new List <KeyValuePair <SteamVR_Utils.RigidTransform, string> > ());
positions.Add(ETrackedDeviceClass.Controller, new List <KeyValuePair <SteamVR_Utils.RigidTransform, string> > ());
positions.Add(ETrackedDeviceClass.GenericTracker, new List <KeyValuePair <SteamVR_Utils.RigidTransform, string> > ());
positions.Add(ETrackedDeviceClass.TrackingReference, new List <KeyValuePair <SteamVR_Utils.RigidTransform, string> > ());
TrackedDevicePose_t[] allPoses = new TrackedDevicePose_t[OpenVR.k_unMaxTrackedDeviceCount];
if (serialNumbers == null)
{
serialNumbers = new string[OpenVR.k_unMaxTrackedDeviceCount];
}
//TODO: TrackingUniverseStanding??
openvr.GetDeviceToAbsoluteTrackingPose(ETrackingUniverseOrigin.TrackingUniverseStanding, 0, allPoses);
for (uint i = 0; i < allPoses.Length; i++)
{
var pose = allPoses[i];
//0:HMD 1:LeftHand 2:RightHand ??
var deviceClass = openvr.GetTrackedDeviceClass(i);
if (pose.bDeviceIsConnected && (deviceClass == ETrackedDeviceClass.HMD || deviceClass == ETrackedDeviceClass.Controller || deviceClass == ETrackedDeviceClass.GenericTracker || deviceClass == ETrackedDeviceClass.TrackingReference))
{
if (serialNumbers[i] == null)
{
serialNumbers[i] = GetTrackerSerialNumber(i);
}
positions[deviceClass].Add(new KeyValuePair <SteamVR_Utils.RigidTransform, string> (new SteamVR_Utils.RigidTransform(pose.mDeviceToAbsoluteTracking), serialNumbers[i]));
}
}
return(positions);
}
public static void ToVRPose(TrackedDevicePose_t trackedPose, out Vector3 position, out Quaternion orientation)
{
Matrix matrix = ToSysMatrix(trackedPose.mDeviceToAbsoluteTracking);
position = matrix.TranslationVector * 100; //m -> cm
orientation = Quaternion.RotationMatrix(matrix);
}
public ActionTransform transform = null; //姿勢
public PoseAction(InputPoseActionData_t Data, OriginSource OriginInfo)
{
Available = Data.bActive;
Origin = OriginInfo;
TrackedDevicePose_t Pose = Data.pose;
SteamVR_Utils.RigidTransform trans = new SteamVR_Utils.RigidTransform(Pose.mDeviceToAbsoluteTracking);
transform = new ActionTransform();
if (OriginInfo != null)
{
transform.deviceid = OriginInfo.DeviceIndex;
}
else
{
transform.deviceid = InvalidDeviceIndex;
}
//右手系・左手系の変換をした
transform.velocity[0] = Pose.vVelocity.v0;
transform.velocity[1] = Pose.vVelocity.v1;
transform.velocity[2] = -Pose.vVelocity.v2;
transform.angularVelocity[0] = -Pose.vAngularVelocity.v0;
transform.angularVelocity[1] = -Pose.vAngularVelocity.v1;
transform.angularVelocity[2] = Pose.vAngularVelocity.v2;
transform.position = trans.pos;
transform.rotation = trans.rot;
}
public Transform GetTransformRaw(uint index)
{
//有効なデバイスか
if (!IsDeviceValid(index))
{
return(null);
}
TrackedDevicePose_t Pose = allDevicePoseRaw[index];
var m = Pose.mDeviceToAbsoluteTracking;
Matrix4x4 mat = new Matrix4x4(
m.m0, m.m4, m.m8, 0,
m.m1, m.m5, m.m9, 0,
m.m2, m.m6, m.m10, 0,
m.m3, m.m7, m.m11, 1
);
Transform res = new Transform();
res.matrix4X4 = mat;
res.deviceid = index;
res.velocity.X = Pose.vVelocity.v0;
res.velocity.Y = Pose.vVelocity.v1;
res.velocity.Z = Pose.vVelocity.v2;
res.angularVelocity.X = Pose.vAngularVelocity.v0;
res.angularVelocity.Y = Pose.vAngularVelocity.v1;
res.angularVelocity.Z = Pose.vAngularVelocity.v2;
res.position = mat.Translation;
res.rotation = Quaternion.CreateFromRotationMatrix(mat);
//res.rotation.W = -res.rotation.W;
return(res);
}
private void Run(FramePreparer framePreparer)
{
float time = 0;
float deltaTime = 1 / 90f;
TrackedDevicePose_t[] gamePoses = new TrackedDevicePose_t[OpenVR.k_unMaxTrackedDeviceCount];
OpenVR.Compositor.GetLastPoses(gamePoses, gamePoses);
Vector3 headPosition = new Vector3(0, 1.5f, 1f);
var stopwatch = Stopwatch.StartNew();
int frameCount = 0;
while (true)
{
time += deltaTime;
var updateParameters = new FrameUpdateParameters(time, deltaTime, gamePoses, headPosition);
var preparedFrame = framePreparer.PrepareFrame(updateParameters);
preparedFrame.Dispose();
frameCount += 1;
if (frameCount == 100)
{
Console.WriteLine(stopwatch.Elapsed.TotalMilliseconds / frameCount);
frameCount = 0;
stopwatch.Restart();
}
}
}
/// <summary>
/// Updates the devices.
/// </summary>
public override void UpdateDevices()
{
// Get indexes of left and right controllers
_leftControllerIndex = -1;
_rightControllerIndex = -1;
int lhIndex = (int)_vrSystem.GetTrackedDeviceIndexForControllerRole(ETrackedControllerRole.LeftHand);
int rhIndex = (int)_vrSystem.GetTrackedDeviceIndexForControllerRole(ETrackedControllerRole.RightHand);
_controllers.Clear();
_trackingReferences.Clear();
// Update all tracked devices
for (uint i = 0; i < OVR.k_unMaxTrackedDeviceCount; i++)
{
TrackedDevicePose_t pose = _devicePoses[i];
// We are interested in valid and connected devices only
if (pose.bDeviceIsConnected && pose.bPoseIsValid)
{
ToVRPose(pose, out Vector3 posePosition, out Quaternion poseOrientation);
ETrackedDeviceClass c = _vrSystem.GetTrackedDeviceClass(i);
// Update controller
if (c == ETrackedDeviceClass.Controller)
{
VRControllerState_t state_t = default(VRControllerState_t);
if (_vrSystem.GetControllerState(i, ref state_t, (uint)System.Runtime.InteropServices.Marshal.SizeOf(state_t)))
{
VRControllerRole role;
if (i == lhIndex)
{
role = VRControllerRole.LeftHand;
_leftControllerIndex = _controllers.Count;
}
else if (i == rhIndex)
{
role = VRControllerRole.RightHand;
_rightControllerIndex = _controllers.Count;
}
else
{
role = VRControllerRole.Undefined;
}
VRControllerState state = new VRControllerState();
state.Update(role, ref state_t, posePosition, poseOrientation);
_controllers.Add(state);
}
}
// Update generic reference (base station etc...)
else if (c == ETrackedDeviceClass.TrackingReference)
{
VRTrackingReference reference = new VRTrackingReference();
reference.Update(posePosition, poseOrientation);
_trackingReferences.Add(reference);
}
}
}
}
private void UpdateControllerState()
{
ETrackingUniverseOrigin tracktype = OpenVR.Compositor.GetTrackingSpace();
TrackedDevicePose_t newposes = new TrackedDevicePose_t();
openvrsystem.GetControllerStateWithPose(tracktype, (uint)index, ref controllerstate, controllerstatesize, ref newposes);
OnNewPoses(newposes);
}
public static void ToVRPose(this TrackedDevicePose_t trackedPose, out VRPose pose)
{
Matrix poseMatrix;
trackedPose.mDeviceToAbsoluteTracking.ToMatrix(out poseMatrix);
pose.Position = poseMatrix.Translation;
pose.Orientation = poseMatrix.Orientation;
}
private void TrackingLoop()
{
try
{
EVRInitError initError = EVRInitError.None;
CVRSystem cvrSystem = OpenVR.Init(ref initError, EVRApplicationType.VRApplication_Utility);
var chaperone = OpenVR.Chaperone;
var quadArea = new HmdQuad_t();
chaperone.GetPlayAreaRect(ref quadArea);
_playArea = quadArea.ToPlayArea();
if (initError != EVRInitError.None)
{
throw new InvalidOperationException($"EVR init erro: {initError}");
}
while (_keepReading)
{
TrackedDevicePose_t[] trackedDevicePoses = new TrackedDevicePose_t[OpenVR.k_unMaxTrackedDeviceCount];
cvrSystem.GetDeviceToAbsoluteTrackingPose(ETrackingUniverseOrigin.TrackingUniverseStanding, 0f, trackedDevicePoses);
for (uint trackedDeviceIndex = 0; trackedDeviceIndex < OpenVR.k_unMaxTrackedDeviceCount; trackedDeviceIndex++)
{
if (cvrSystem.IsTrackedDeviceConnected(trackedDeviceIndex))
{
ETrackedDeviceClass deviceClass = cvrSystem.GetTrackedDeviceClass(trackedDeviceIndex);
if (true)
{
VRControllerState_t controllerState = new VRControllerState_t();
cvrSystem.GetControllerState(1, ref controllerState, (uint)System.Runtime.InteropServices.Marshal.SizeOf(typeof(VRControllerState_t)));
ETrackingResult trackingResult = trackedDevicePoses[trackedDeviceIndex].eTrackingResult;
bool trigger = controllerState.rAxis1.x > 0.9f;
bool menuButton = (controllerState.ulButtonPressed & (1ul << (int)EVRButtonId.k_EButton_ApplicationMenu)) != 0;
bool gripButton = (controllerState.ulButtonPressed & (1ul << (int)EVRButtonId.k_EButton_Grip)) != 0;
if (trackingResult == ETrackingResult.Running_OK)
{
HmdMatrix34_t trackingMatrix = trackedDevicePoses[trackedDeviceIndex].mDeviceToAbsoluteTracking;
Vector3 speedVector = trackedDevicePoses[trackedDeviceIndex].vVelocity.ToVelocityVector();
Vector3 position = trackingMatrix.ToPositionVector();
DeviceTrackingData trackingUpdate = new DeviceTrackingData((int)trackedDeviceIndex, deviceClass.ToString(), position, trackingMatrix.ToRotationQuaternion());
NewPoseUpdate?.Invoke(this, trackingUpdate);
}
}
}
}
Thread.Sleep(UpdatedInterval);
}
}
catch (Exception e)
{
Console.WriteLine($"Error: {e}");
}
finally
{
OpenVR.Shutdown();
}
}
void OnNewPoses(TrackedDevicePose_t[] poses)
{
if (!poses [0].bPoseIsValid)
{
requireCalibration = true;
}
if (autoCalibration)
{
Recalibrate(poses, canAdjust);
}
if (!controllerRegistered)
{
leftId = SteamVR_Controller.GetDeviceIndex(SteamVR_Controller.DeviceRelation.Leftmost);
rightId = SteamVR_Controller.GetDeviceIndex(SteamVR_Controller.DeviceRelation.Rightmost);
}
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");
calibrationError = true;
}
else if (rightPose.mDeviceToAbsoluteTracking.m7 < -0.01f)
{
Debug.Log("Right controller calibration error");
calibrationError = true;
}
else
{
calibrationError = false;
}
}
// //Get & display chaperone center point coordinates
// CVRChaperoneSetup setup = OpenVR.ChaperoneSetup;
// setup.GetWorkingStandingZeroPoseToRawTrackingPose (ref centerMatrix);
// Debug.Log ("Center Coordinate : " + centerMatrix.m3 + "," + centerMatrix.m7 + "," + centerMatrix.m11);
}
public void DoPrework(DeviceContext context, TrackedDevicePose_t[] poses)
{
for (int i = 0; i < MaxDeviceCount; ++i)
{
TrackedDevicePose_t pose = poses[i];
Matrix objectToWorldSpaceTransform = pose.bPoseIsValid ? pose.mDeviceToAbsoluteTracking.Convert() : Matrix.Zero;
objectSpaceToWorldTransformBufferManagers[i].Update(context, objectToWorldSpaceTransform);
}
}
//
public override int GetInputState(ref XDevicePlugin.ControllerState state)
{
if (m_Device.connected)
{
Vector2 v2;
//
v2 = m_Device.GetAxis(EVRButtonId.k_EButton_Axis1);
state.axes[(int)ControllerAxis.PrimaryTrigger] = v2.x;
v2 = m_Device.GetAxis(EVRButtonId.k_EButton_Axis0);
state.axes[(int)ControllerAxis.PrimaryThumbX] = v2.x;
state.axes[(int)ControllerAxis.PrimaryThumbY] = v2.y;
//
state.buttons = 0;
if (m_Device.GetHairTrigger())
{
state.buttons |= (uint)XimmerseButton.Trigger;
}
if (m_Device.GetPress(SteamVR_Controller.ButtonMask.Grip))
{
state.buttons |= (uint)XimmerseButton.Grip;
}
if (m_Device.GetTouch(SteamVR_Controller.ButtonMask.Touchpad))
{
state.buttons |= (uint)XimmerseButton.Touch;
}
if (m_Device.GetPress(SteamVR_Controller.ButtonMask.Touchpad))
{
state.buttons |= (uint)XimmerseButton.Click;
}
if (m_Device.GetPress(SteamVR_Controller.ButtonMask.ApplicationMenu))
{
state.buttons |= (uint)XimmerseButton.App;
}
if (m_Device.GetPress(SteamVR_Controller.ButtonMask.System))
{
state.buttons |= (uint)XimmerseButton.Home;
}
//
TrackedDevicePose_t pose = m_UseOnNewPoses?m_Pose:m_Device.GetPose();
XDevicePlugin.SetInt(handle, XDevicePlugin.kField_TrackingResultInt, (int)(pose.bPoseIsValid?TrackingResult.PoseTracked:TrackingResult.NotTracked));
SteamVR_Utils.RigidTransform rt = new SteamVR_Utils.RigidTransform(pose.mDeviceToAbsoluteTracking);
Vector3 v3 = rt.pos;
state.position[0] = v3.x;
state.position[1] = v3.y;
state.position[2] = -v3.z;
Quaternion q = rt.rot;
state.rotation[0] = -q.x;
state.rotation[1] = -q.y;
state.rotation[2] = q.z;
state.rotation[3] = q.w;
//
}
return(0);
}
public DeviceInfo(SteamVR_Utils.RigidTransform rigidTransform, string serial, TrackedDevicePose_t result, ETrackedDeviceClass deviceClass)
{
transform = rigidTransform;
serialNumber = serial;
trackingStatus = result;
trackedDeviceClass = deviceClass;
updateValues();
}
public void OnLateUpdate()
{
dispatchOpenVREvents();
EVRCompositorError compositorError = OpenVR.Compositor.WaitGetPoses(VRManager.RenderPoses, VRManager.GamePoses);
if (compositorError != EVRCompositorError.None)
{
throw new Exception("OpenVr error: " + compositorError.ToString());
}
OpenVR.System.GetDeviceToAbsoluteTrackingPose(ETrackingUniverseOrigin.TrackingUniverseStanding, Utils.CalculatePredictedSecondsToPhotons(), VRManager.DevicePoses);
SteamVR_Events.NewPoses.Send(VRManager.DevicePoses);
for (int i = 0; i < VRManager.DevicePoses.Length; i++)
{
TrackedDevicePose_t devicePose = VRManager.DevicePoses[i];
if (!devicePose.bDeviceIsConnected || !devicePose.bPoseIsValid)
{
continue;
}
SteamVR_Utils.RigidTransform rigidTransform = new SteamVR_Utils.RigidTransform(devicePose.mDeviceToAbsoluteTracking);
ETrackedDeviceClass deviceType = OpenVR.System.GetTrackedDeviceClass((uint)i);
if (deviceType == ETrackedDeviceClass.HMD)
{
Head.transform.localPosition = rigidTransform.pos;
Head.transform.localRotation = rigidTransform.rot;
Head.Velocity = fromHmdVector3_t(devicePose.vVelocity);
Head.AngularVelocity = fromHmdVector3_t(devicePose.vAngularVelocity);
Head.DeviceIndex = (uint)i;
}
else if (deviceType == ETrackedDeviceClass.Controller)
{
ETrackedControllerRole role = OpenVR.System.GetControllerRoleForTrackedDeviceIndex((uint)i);
if (role == ETrackedControllerRole.LeftHand)
{
LeftController.transform.localPosition = rigidTransform.pos;
LeftController.transform.localRotation = rigidTransform.rot;
LeftController.Velocity = fromHmdVector3_t(devicePose.vVelocity);
LeftController.AngularVelocity = fromHmdVector3_t(devicePose.vAngularVelocity);
LeftController.DeviceIndex = (uint)i;
}
else if (role == ETrackedControllerRole.RightHand)
{
RightController.transform.localPosition = rigidTransform.pos;
RightController.transform.localRotation = rigidTransform.rot;
RightController.Velocity = fromHmdVector3_t(devicePose.vVelocity);
RightController.AngularVelocity = fromHmdVector3_t(devicePose.vAngularVelocity);
RightController.DeviceIndex = (uint)i;
}
}
}
}
public void Update()
{
if (!playing)
{
return;
}
if (steamVrObject == null)
{
return;
}
if (clipDuration == 0f)
{
return;
}
float iterCount = Time.time / clipDuration;
float iterPosition = Time.time / (iterCount);
iterPosition = 9999;
if (currentClip == null)
{
currentClip = linkedList.First;
}
else
{
currentClip = currentClip.Next;
}
if (currentClip == null)
{
return;
}
TrackedDevicePose_t[] poses = new TrackedDevicePose_t[10];
TrackedDevicePose_t pose = new TrackedDevicePose_t();
pose.mDeviceToAbsoluteTracking = new HmdMatrix34_t();
pose.mDeviceToAbsoluteTracking.m0 = currentClip.Value.m0;
pose.mDeviceToAbsoluteTracking.m1 = currentClip.Value.m1;
pose.mDeviceToAbsoluteTracking.m2 = currentClip.Value.m2;
pose.mDeviceToAbsoluteTracking.m3 = currentClip.Value.m3;
pose.mDeviceToAbsoluteTracking.m4 = currentClip.Value.m4;
pose.mDeviceToAbsoluteTracking.m5 = currentClip.Value.m5;
pose.mDeviceToAbsoluteTracking.m6 = currentClip.Value.m6;
pose.mDeviceToAbsoluteTracking.m7 = currentClip.Value.m7;
pose.mDeviceToAbsoluteTracking.m8 = currentClip.Value.m8;
pose.mDeviceToAbsoluteTracking.m9 = currentClip.Value.m9;
pose.mDeviceToAbsoluteTracking.m10 = currentClip.Value.m10;
pose.mDeviceToAbsoluteTracking.m11 = currentClip.Value.m11;
pose.bDeviceIsConnected = true;
pose.bPoseIsValid = true;
poses[(int)steamVrObject.index] = pose;
steamVrObject.OnNewPoses(poses);
lastControllerState = currentClip.Value.controllerState;
}
public abstract void GetDeviceToAbsoluteTrackingPose(TrackingUniverseOrigin eOrigin,float fPredictedSecondsToPhotonsFromNow,TrackedDevicePose_t [] pTrackedDevicePoseArray);
public abstract bool GetControllerStateWithPose(TrackingUniverseOrigin eOrigin,uint unControllerDeviceIndex,ref VRControllerState_t pControllerState,ref TrackedDevicePose_t pTrackedDevicePose);
public abstract VRCompositorError WaitGetPoses(TrackedDevicePose_t [] pRenderPoseArray,TrackedDevicePose_t [] pGamePoseArray);
public override bool PollNextEventWithPose(TrackingUniverseOrigin eOrigin,ref VREvent_t pEvent,ref TrackedDevicePose_t pTrackedDevicePose)
{
CheckIfUsable();
bool result = VRNativeEntrypoints.VR_IVRSystem_PollNextEventWithPose(m_pVRSystem,eOrigin,ref pEvent,ref pTrackedDevicePose);
return result;
}
public override void GetDeviceToAbsoluteTrackingPose(TrackingUniverseOrigin eOrigin,float fPredictedSecondsToPhotonsFromNow,TrackedDevicePose_t [] pTrackedDevicePoseArray)
{
CheckIfUsable();
VRNativeEntrypoints.VR_IVRSystem_GetDeviceToAbsoluteTrackingPose(m_pVRSystem,eOrigin,fPredictedSecondsToPhotonsFromNow,pTrackedDevicePoseArray,(uint) pTrackedDevicePoseArray.Length);
}
public override bool GetControllerStateWithPose(TrackingUniverseOrigin eOrigin,uint unControllerDeviceIndex,ref VRControllerState_t pControllerState,ref TrackedDevicePose_t pTrackedDevicePose)
{
CheckIfUsable();
bool result = VRNativeEntrypoints.VR_IVRSystem_GetControllerStateWithPose(m_pVRSystem,eOrigin,unControllerDeviceIndex,ref pControllerState,ref pTrackedDevicePose);
return result;
}
public abstract EVRCompositorError GetLastPoses(TrackedDevicePose_t [] pRenderPoseArray,TrackedDevicePose_t [] pGamePoseArray);
public abstract void ApplyTransform(ref TrackedDevicePose_t pOutputPose,IntPtr trackedDevicePose,IntPtr transform);
public override void WaitGetPoses(TrackedDevicePose_t [] pPoseArray)
{
CheckIfUsable();
VRNativeEntrypoints.VR_IVRCompositor_WaitGetPoses(m_pVRCompositor,pPoseArray,(uint) pPoseArray.Length);
}
public abstract void WaitGetPoses(TrackedDevicePose_t [] pPoseArray);
public override void ApplyTransform(ref TrackedDevicePose_t pOutputPose,ref TrackedDevicePose_t pTrackedDevicePose,ref HmdMatrix34_t pTransform)
{
CheckIfUsable();
VRNativeEntrypoints.VR_IVRSystem_ApplyTransform(m_pVRSystem,ref pOutputPose,ref pTrackedDevicePose,ref pTransform);
}
public abstract bool PollNextEventWithPose(TrackingUniverseOrigin eOrigin,ref VREvent_t pEvent,ref TrackedDevicePose_t pTrackedDevicePose);
public override VRCompositorError WaitGetPoses(TrackedDevicePose_t [] pRenderPoseArray,TrackedDevicePose_t [] pGamePoseArray)
{
CheckIfUsable();
VRCompositorError result = VRNativeEntrypoints.VR_IVRCompositor_WaitGetPoses(m_pVRCompositor,pRenderPoseArray,(uint) pRenderPoseArray.Length,pGamePoseArray,(uint) pGamePoseArray.Length);
return result;
}
public abstract void ApplyTransform(ref TrackedDevicePose_t pOutputPose,ref TrackedDevicePose_t pTrackedDevicePose,ref HmdMatrix34_t pTransform);
