FreePIE-OSVR/Tracker.cs at master · thomasgauthier/FreePIE-OSVR · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
using System;
using OSVR.ClientKit;

namespace OSVRFreePIE
{
    public class Tracker
    {
        private PoseInterface poseInterface;
        private PositionInterface positionInterface;
        private OrientationInterface orientationInterface;
        private Vec3 positionValue;
        private Quaternion orientationValue;
        private double rollValue;
        private double pitchValue;
        private double yawValue;

        public Tracker(ClientContext context, String path)
        {
            rollValue = pitchValue = yawValue = 0.0f;

            poseInterface = context.GetPoseInterface(path);
            positionInterface = context.GetPositionInterface(path);
            orientationInterface = context.GetOrientationInterface(path);

            poseInterface.StateChanged += PoseInterface_StateChanged;
            positionInterface.StateChanged += PositionInterface_StateChanged;
            orientationInterface.StateChanged += OrientationInterface_StateChanged;
        }

        private void OrientationInterface_StateChanged(object sender, TimeValue timestamp, int sensor, Quaternion report)
        {
            orientationValue = report;
            updateRollPitchYaw();
        }

        private void PositionInterface_StateChanged(object sender, TimeValue timestamp, int sensor, Vec3 report)
        {
            positionValue = report;
        }

        private void PoseInterface_StateChanged(object sender, TimeValue timestamp, int sensor, Pose3 report)
        {
            orientationValue = report.rotation;
            positionValue = report.translation;
            updateRollPitchYaw();
        }

        private void updateRollPitchYaw()
        {
            var q = new double[] {
                orientationValue.w,
                orientationValue.z,
                orientationValue.x,
                orientationValue.y
             };

            rollValue = -Math.Atan2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2]));
            pitchValue = -Math.Asin(2 * (q[0] * q[2] - q[3] * q[1]));
            yawValue = -Math.Atan2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] * q[2] + q[3] * q[3]));
        }

        public Vec3 position
        {
            get
            {
                return positionValue;
            }
        }

        public Quaternion orientation
        {
            get
            {
                return orientationValue;
            }
        }

        public double x
        {
            get
            {
                return positionValue.x;
            }
        }

        public double y
        {
            get
            {
                return positionValue.y;
            }
        }

        public double z
        {
            get
            {
                return positionValue.z;
            }
        }

        public double roll
        {
            get
            {
                return rollValue;
            }
        }

        public double pitch
        {
            get
            {
                return pitchValue;
            }
        }

        public double yaw
        {
            get
            {
                return yawValue;
            }
        }
    }
}