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;
}
}
/// <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);
}
public void Update()
{
if (Time.frameCount != this.prevFrameCount)
{
this.prevFrameCount = Time.frameCount;
this.prevState = this.state;
CVRSystem system = OpenVR.System;
if (system != null)
{
this.valid = system.GetControllerStateWithPose(SteamVR_Render.instance.trackingSpace, this.index, ref this.state, (uint)Marshal.SizeOf(typeof(VRControllerState_t)), ref this.pose);
this.UpdateHairTrigger();
}
}
}
public override int GetInputState(ref XDevicePlugin.ControllerState state)
{
CVRSystem system = OpenVR.System;
if (system != null)
{
int i;
if (/*valid=*/ system.GetControllerStateWithPose(SteamVR_Render.instance.trackingSpace, m_Index, ref m_State, ref m_Pose))
{
//
i = 0;
state.axes[i++] = m_State.rAxis1.x; // trigger
++i;
state.axes[i++] = m_State.rAxis0.x;
state.axes[i++] = m_State.rAxis0.y;
++i;
++i;
if (m_InputTransform != null)
{
state.buttons = TransformButton(m_InputTransform, m_State.ulButtonPressed, m_State.ulButtonTouched);
}
else
{
// You needs an InputTransform to convert ulong buttons to uint buttons.
state.buttons = 0;
}
//
SteamVR_Utils.RigidTransform t = new SteamVR_Utils.RigidTransform(m_Pose.mDeviceToAbsoluteTracking);
for (i = 0; i < 3; ++i)
{
state.position[i] = t.pos[i];
}
state.position[i] = 1.0f;
for (i = 0; i < 4; ++i)
{
state.rotation[i] = t.rot[i];
}
return(0);
}
}
return(-1);
}
public void Update(CVRSystem system, int index, Matrix transform)
{
Index = index;
if (Index < 0)
{
IsValid = false;
m_Pose.bPoseIsValid = false;
return;
}
m_PreviousState = m_State;
IsValid = system.GetControllerStateWithPose(ETrackingUniverseOrigin.TrackingUniverseStanding, (uint)index, ref m_State, (uint)Marshal.SizeOf(typeof(VRControllerState_t)), ref m_Pose);
transform = Matrix.Invert(transform);
var parent = Components.Camera.Main.Transform.Parent;
//if (parent != null)
//transform = parent.m_WorldMatrix * transform;
transform.Decompose(out m_Scale, out m_Rotation, out m_Position);
}
public static bool GetControllerStateWithPose(this CVRSystem system, ETrackingUniverseOrigin eOrigin, uint unControllerDeviceIndex, out VRControllerState_t pControllerState, out TrackedDevicePose_t pTrackedDevicePose)
{
pControllerState = default(VRControllerState_t);
pTrackedDevicePose = default(TrackedDevicePose_t);
return(system.GetControllerStateWithPose(eOrigin, unControllerDeviceIndex, ref pControllerState, CONTROLLER_STATE_SIZE, ref pTrackedDevicePose));
}
private async void ParseTrackingFrame()
{
try
{
for (uint id = 0; id < OpenVR.k_unMaxTrackedDeviceCount; id++)
{
if (vr_pointer != null)
{
TrackedDevicePose_t[] trackedDevicePose = new TrackedDevicePose_t[OpenVR.k_unMaxTrackedDeviceCount];
if (!vr_pointer.IsTrackedDeviceConnected(id))
{
continue;
}
VRControllerState_t controllState = new VRControllerState_t();
ETrackedDeviceClass trackedDeviceClass = vr_pointer.GetTrackedDeviceClass(id);
switch (trackedDeviceClass)
{
case ETrackedDeviceClass.Controller:
vr_pointer.GetControllerStateWithPose(ETrackingUniverseOrigin.TrackingUniverseStanding, id, ref controllState, OpenVR.k_unMaxTrackedDeviceCount, ref trackedDevicePose[id]);
HmdVector3_t position = GetPosition(trackedDevicePose[id].mDeviceToAbsoluteTracking); // devicePose->mDeviceToAbsoluteTracking);
HmdVector3_t rotation = GetRotationEuler(trackedDevicePose[id].mDeviceToAbsoluteTracking);
int positionControllerX = (int)position.v0;
int positionControllerY = (int)position.v1;
int positionControllerZ = (int)position.v2 * (-1);
int rotationControllerX = (int)rotation.v0 + 180;
int rotationControllerY = (int)rotation.v1;
int rotationControllerZ = (int)rotation.v2;
if (ControlViewModel.teleporOffset[0] != -1)
{
ControlViewModel.transOffSetControllerX = ControlViewModel.teleporOffset[0];
ControlViewModel.transOffSetControllerY = ControlViewModel.teleporOffset[1];
ControlViewModel.transOffSetControllerZ = ControlViewModel.teleporOffset[2];
ControlViewModel.rotOffSetControllerZ = ControlViewModel.teleporOffset[3];
}
ETrackedControllerRole result = vr_pointer.GetControllerRoleForTrackedDeviceIndex(id);
if (result != ETrackedControllerRole.LeftHand && lastController != ETrackedControllerRole.LeftHand)
{
RaiseControllerEvent(new TrackingChangedEventArgs(false), Tracker.LeftController);
}
if (result != ETrackedControllerRole.RightHand && lastController != ETrackedControllerRole.RightHand)
{
RaiseControllerEvent(new TrackingChangedEventArgs(false), Tracker.RightController);
}
switch (result)
{
case ETrackedControllerRole.Invalid:
break;
case ETrackedControllerRole.LeftHand:
if (communicationProtocolUdp == false)
{
MessageBuffer mb2 = new MessageBuffer();
someThingTracked = true;
int type = 101;
mb2.add(type);
mb2.add(userId);
int handid = 1;
mb2.add(handid);
mb2.add(positionControllerX + ControlViewModel.transOffSetControllerX);
mb2.add(positionControllerY + ControlViewModel.transOffSetControllerY);
mb2.add(positionControllerZ + ControlViewModel.transOffSetControllerZ);
mb2.add(rotationControllerX);
mb2.add(rotationControllerY);
mb2.add(rotationControllerZ);
mb2.add(ControlViewModel.rotOffSetControllerZ);
RaiseControllerEvent(new TrackingChangedEventArgs(true), Tracker.LeftController);
Message msg = new Message(mb2, Message.MessagesType.COVISE_MESSAGE_VRB_MESSAGE);
lock (tcpSocketManager.send_msg(msg))
{
tcpSocketManager.send_msg(msg);
}
Thread.Sleep(20);
break;
}
break;
case ETrackedControllerRole.RightHand:
if (communicationProtocolUdp == false)
{
MessageBuffer mb2 = new MessageBuffer();
someThingTracked = true;
int type = 101;
mb2.add(type);
mb2.add(userId);
int handid = 2;
mb2.add(handid);
mb2.add(positionControllerX + ControlViewModel.transOffSetControllerX);
mb2.add(positionControllerY + ControlViewModel.transOffSetControllerY);
mb2.add(positionControllerZ + ControlViewModel.transOffSetControllerZ);
mb2.add(rotationControllerX);
mb2.add(rotationControllerY);
mb2.add(rotationControllerZ);
mb2.add(ControlViewModel.rotOffSetControllerZ);
Message msg = new Message(mb2, Message.MessagesType.COVISE_MESSAGE_VRB_MESSAGE);
RaiseControllerEvent(new TrackingChangedEventArgs(true), Tracker.RightController);
lock (tcpSocketManager.send_msg(msg))
{
tcpSocketManager.send_msg(msg);
}
Thread.Sleep(20);
break;
}
break;
}
break;
}
}
}
}
catch (Exception e)
{
log.Warn("ParseTrackingFrame in Controller Tracking wurde beendet!" + e);
RaiseControllerEvent(new TrackingChangedEventArgs(false), Tracker.LeftController);
RaiseControllerEvent(new TrackingChangedEventArgs(false), Tracker.RightController);
}
}
private void VRInit()
{
// init
var error = EVRInitError.None;
system = OpenVR.Init(ref error);
if (error != EVRInitError.None)
{
throw new Exception();
}
OpenVR.GetGenericInterface(OpenVR.IVRSystem_Version, ref error);
if (error != EVRInitError.None)
{
throw new Exception();
}
TrackedDevicePose_t[] m_rTrackedDevicePose = new TrackedDevicePose_t[OpenVR.k_unMaxTrackedDeviceCount];
TrackedDevicePose_t TrackedDevicePose = new TrackedDevicePose_t();
VRControllerState_t controllerState = new VRControllerState_t();
while (true)
{
system.GetDeviceToAbsoluteTrackingPose(ETrackingUniverseOrigin.TrackingUniverseStanding, 0, m_rTrackedDevicePose);
for (uint unDevice = 0; unDevice < OpenVR.k_unMaxTrackedDeviceCount; unDevice++)
{
if (!system.IsTrackedDeviceConnected(unDevice))
{
continue;
}
HmdVector3_t position;
HmdQuaternion_t quaternion;
// Get what type of device it is and work with its data
ETrackedDeviceClass trackedDeviceClass = system.GetTrackedDeviceClass(unDevice);
ETrackedControllerRole trackedControllerRole = system.GetControllerRoleForTrackedDeviceIndex(unDevice);
ETrackingResult eTrackingResult;
switch (trackedDeviceClass)
{
case ETrackedDeviceClass.HMD:
// get the position and rotation
position = GetPosition(m_rTrackedDevicePose[unDevice].mDeviceToAbsoluteTracking);
quaternion = GetRotation(m_rTrackedDevicePose[unDevice].mDeviceToAbsoluteTracking);
// for printing some more info to the user about the state of the device/pose
eTrackingResult = m_rTrackedDevicePose[unDevice].eTrackingResult;
// print the tracking data
Dispatcher.Invoke(DispatcherPriority.Normal, new Action(delegate {
HmdIndex.Text = String.Format("{0}", unDevice);
HmdVectorX.Text = String.Format("{0:F4}", position.v0);
HmdVectorY.Text = String.Format("{0:F4}", position.v1);
HmdVectorZ.Text = String.Format("{0:F4}", position.v2);
HmdQuaternionX.Text = String.Format("{0:F4}", quaternion.x);
HmdQuaternionY.Text = String.Format("{0:F4}", quaternion.y);
HmdQuaternionZ.Text = String.Format("{0:F4}", quaternion.z);
HmdQuaternionW.Text = String.Format("{0:F4}", quaternion.w);
HmdState.Text = GetDeviceState(eTrackingResult);
}));
break;
case ETrackedDeviceClass.Controller:
// get the position and rotation
position = GetPosition(m_rTrackedDevicePose[unDevice].mDeviceToAbsoluteTracking);
quaternion = GetRotation(m_rTrackedDevicePose[unDevice].mDeviceToAbsoluteTracking);
// for printing some more info to the user about the state of the device/pose
eTrackingResult = m_rTrackedDevicePose[unDevice].eTrackingResult;
// get Controllers info
system.GetControllerStateWithPose(ETrackingUniverseOrigin.TrackingUniverseStanding, unDevice, ref controllerState, (uint)Marshal.SizeOf(controllerState), ref TrackedDevicePose);
switch (trackedControllerRole)
{
case ETrackedControllerRole.LeftHand:
// print the tracking data
Dispatcher.Invoke(DispatcherPriority.Normal, new Action(delegate {
CLIndex.Text = String.Format("{0}", unDevice);
CLVectorX.Text = String.Format("{0:F4}", position.v0);
CLVectorY.Text = String.Format("{0:F4}", position.v1);
CLVectorZ.Text = String.Format("{0:F4}", position.v2);
CLQuaternionX.Text = String.Format("{0:F4}", quaternion.x);
CLQuaternionY.Text = String.Format("{0:F4}", quaternion.y);
CLQuaternionZ.Text = String.Format("{0:F4}", quaternion.z);
CLQuaternionW.Text = String.Format("{0:F4}", quaternion.w);
CLState.Text = GetDeviceState(eTrackingResult);
CLulPressed.Text = String.Format("{0}", controllerState.ulButtonPressed);
CLulTouched.Text = String.Format("{0}", controllerState.ulButtonTouched);
CLAxis0X.Text = String.Format("{0:F4}", controllerState.rAxis0.x);
CLAxis0Y.Text = String.Format("{0:F4}", controllerState.rAxis0.y);
CLAxis1X.Text = String.Format("{0:F4}", controllerState.rAxis1.x);
CLAxis1Y.Text = String.Format("{0:F4}", controllerState.rAxis1.y);
CLAxis2X.Text = String.Format("{0:F4}", controllerState.rAxis2.x);
CLAxis2Y.Text = String.Format("{0:F4}", controllerState.rAxis2.y);
CLAxis3X.Text = String.Format("{0:F4}", controllerState.rAxis3.x);
CLAxis3Y.Text = String.Format("{0:F4}", controllerState.rAxis3.y);
CLAxis4X.Text = String.Format("{0:F4}", controllerState.rAxis4.x);
CLAxis4Y.Text = String.Format("{0:F4}", controllerState.rAxis4.y);
}));
break;
case ETrackedControllerRole.RightHand:
// print the tracking data
Dispatcher.Invoke(DispatcherPriority.Normal, new Action(delegate {
CRIndex.Text = String.Format("{0}", unDevice);
CRVectorX.Text = String.Format("{0:F4}", position.v0);
CRVectorY.Text = String.Format("{0:F4}", position.v1);
CRVectorZ.Text = String.Format("{0:F4}", position.v2);
CRQuaternionX.Text = String.Format("{0:F4}", quaternion.x);
CRQuaternionY.Text = String.Format("{0:F4}", quaternion.y);
CRQuaternionZ.Text = String.Format("{0:F4}", quaternion.z);
CRQuaternionW.Text = String.Format("{0:F4}", quaternion.w);
CRState.Text = GetDeviceState(eTrackingResult);
CRulPressed.Text = String.Format("{0}", controllerState.ulButtonPressed);
CRulTouched.Text = String.Format("{0}", controllerState.ulButtonTouched);
CRAxis0X.Text = String.Format("{0:F4}", controllerState.rAxis0.x);
CRAxis0Y.Text = String.Format("{0:F4}", controllerState.rAxis0.y);
CRAxis1X.Text = String.Format("{0:F4}", controllerState.rAxis1.x);
CRAxis1Y.Text = String.Format("{0:F4}", controllerState.rAxis1.y);
CRAxis2X.Text = String.Format("{0:F4}", controllerState.rAxis2.x);
CRAxis2Y.Text = String.Format("{0:F4}", controllerState.rAxis2.y);
CRAxis3X.Text = String.Format("{0:F4}", controllerState.rAxis3.x);
CRAxis3Y.Text = String.Format("{0:F4}", controllerState.rAxis3.y);
CRAxis4X.Text = String.Format("{0:F4}", controllerState.rAxis4.x);
CRAxis4Y.Text = String.Format("{0:F4}", controllerState.rAxis4.y);
}));
break;
}
break;
case ETrackedDeviceClass.GenericTracker:
system.GetControllerStateWithPose(ETrackingUniverseOrigin.TrackingUniverseStanding, unDevice, ref controllerState, (uint)Marshal.SizeOf(controllerState), ref TrackedDevicePose);
// get the position and rotation
position = GetPosition(TrackedDevicePose.mDeviceToAbsoluteTracking);
quaternion = GetRotation(TrackedDevicePose.mDeviceToAbsoluteTracking);
// for printing some more info to the user about the state of the device/pose
eTrackingResult = m_rTrackedDevicePose[unDevice].eTrackingResult;
// print the tracking data
Dispatcher.Invoke(DispatcherPriority.Normal, new Action(delegate {
TrackerIndex.Text = String.Format("{0}", unDevice);
TrackerVectorX.Text = String.Format("{0:F4}", position.v0);
TrackerVectorY.Text = String.Format("{0:F4}", position.v1);
TrackerVectorZ.Text = String.Format("{0:F4}", position.v2);
TrackerQuaternionX.Text = String.Format("{0:F4}", quaternion.x);
TrackerQuaternionY.Text = String.Format("{0:F4}", quaternion.y);
TrackerQuaternionZ.Text = String.Format("{0:F4}", quaternion.z);
TrackerQuaternionW.Text = String.Format("{0:F4}", quaternion.w);
TrackerState.Text = GetDeviceState(eTrackingResult);
}));
break;
}
Thread.Sleep(10);
}
}
}
