matrixfurry.com
The open source OpenXR runtime
1// Copyright 2020-2022, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Controller remote driver.
6 * @author Jakob Bornecrantz <jakob@collabora.com>
7 * @ingroup drv_remote
8 */
9
10#include "r_internal.h"
11
12#include "os/os_time.h"
13
14#include "util/u_var.h"
15#include "util/u_misc.h"
16#include "util/u_debug.h"
17#include "util/u_device.h"
18#include "util/u_hand_tracking.h"
19
20#include "vive/vive_bindings.h"
21
22#include "math/m_api.h"
23
24#include "util/u_hand_simulation.h"
25
26#include <stdio.h>
27
28
29/*
30 *
31 * Functions
32 *
33 */
34
35static inline struct r_device *
36r_device(struct xrt_device *xdev)
37{
38 return (struct r_device *)xdev;
39}
40
41static void
42r_device_destroy(struct xrt_device *xdev)
43{
44 struct r_device *rd = r_device(xdev);
45
46 // Remove the variable tracking.
47 u_var_remove_root(rd);
48
49 // Free this device with the helper.
50 u_device_free(&rd->base);
51}
52
53static xrt_result_t
54r_device_update_inputs(struct xrt_device *xdev)
55{
56 struct r_device *rd = r_device(xdev);
57 struct r_hub *r = rd->r;
58
59 uint64_t now = os_monotonic_get_ns();
60 struct r_remote_controller_data *latest = rd->is_left ? &r->latest.left : &r->latest.right;
61
62 // TODO: refactor those loops into one
63 if (!latest->active) {
64 for (uint32_t i = 0; i < xdev->input_count; i++) {
65 xdev->inputs[i].active = false;
66 xdev->inputs[i].timestamp = now;
67 U_ZERO(&xdev->inputs[i].value);
68 }
69 return XRT_SUCCESS;
70 }
71
72 for (uint32_t i = 0; i < xdev->input_count; i++) {
73 xdev->inputs[i].active = true;
74 xdev->inputs[i].timestamp = now;
75 }
76
77 // clang-format off
78 xdev->inputs[0].value.boolean = latest->system_click;
79 xdev->inputs[1].value.boolean = latest->system_touch;
80 xdev->inputs[2].value.boolean = latest->a_click;
81 xdev->inputs[3].value.boolean = latest->a_touch;
82 xdev->inputs[4].value.boolean = latest->b_click;
83 xdev->inputs[5].value.boolean = latest->b_touch;
84 xdev->inputs[6].value.vec1 = latest->squeeze_value;
85 xdev->inputs[7].value.vec1 = latest->squeeze_force;
86 xdev->inputs[8].value.boolean = latest->trigger_click;
87 xdev->inputs[9].value.vec1 = latest->trigger_value;
88 xdev->inputs[10].value.boolean = latest->trigger_touch;
89 xdev->inputs[11].value.vec2 = latest->thumbstick;
90 xdev->inputs[12].value.boolean = latest->thumbstick_click;
91 xdev->inputs[13].value.boolean = latest->thumbstick_touch;
92 xdev->inputs[14].value.vec2 = latest->trackpad;
93 xdev->inputs[15].value.vec1 = latest->trackpad_force;
94 xdev->inputs[16].value.boolean = latest->trackpad_touch;
95 // clang-format on
96
97 return XRT_SUCCESS;
98}
99
100static xrt_result_t
101r_device_get_tracked_pose(struct xrt_device *xdev,
102 enum xrt_input_name name,
103 int64_t at_timestamp_ns,
104 struct xrt_space_relation *out_relation)
105{
106 struct r_device *rd = r_device(xdev);
107 struct r_hub *r = rd->r;
108
109 if (name != XRT_INPUT_INDEX_AIM_POSE && name != XRT_INPUT_INDEX_GRIP_POSE &&
110 name != XRT_INPUT_GENERIC_PALM_POSE) {
111 U_LOG_XDEV_UNSUPPORTED_INPUT(&rd->base, u_log_get_global_level(), name);
112 return XRT_ERROR_INPUT_UNSUPPORTED;
113 }
114
115 struct r_remote_controller_data *latest = rd->is_left ? &r->latest.left : &r->latest.right;
116
117 /*
118 * It's easier to reason about angular velocity if it's controlled in
119 * body space, but the angular velocity returned in the relation is in
120 * the base space.
121 */
122 math_quat_rotate_derivative(&latest->pose.orientation, &latest->angular_velocity,
123 &out_relation->angular_velocity);
124
125 out_relation->pose = latest->pose;
126 out_relation->linear_velocity = latest->linear_velocity;
127
128 if (latest->active) {
129 out_relation->relation_flags = (enum xrt_space_relation_flags)(
130 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT |
131 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT |
132 XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
133 } else {
134 out_relation->relation_flags = 0;
135 }
136
137 return XRT_SUCCESS;
138}
139
140static xrt_result_t
141r_device_get_hand_tracking(struct xrt_device *xdev,
142 enum xrt_input_name name,
143 int64_t requested_timestamp_ns,
144 struct xrt_hand_joint_set *out_value,
145 int64_t *out_timestamp_ns)
146{
147 struct r_device *rd = r_device(xdev);
148 struct r_hub *r = rd->r;
149
150 if (name != XRT_INPUT_HT_CONFORMING_LEFT && name != XRT_INPUT_HT_CONFORMING_RIGHT) {
151 U_LOG_XDEV_UNSUPPORTED_INPUT(&rd->base, u_log_get_global_level(), name);
152 return XRT_ERROR_INPUT_UNSUPPORTED;
153 }
154
155 struct r_remote_controller_data *latest = rd->is_left ? &r->latest.left : &r->latest.right;
156
157 struct u_hand_tracking_curl_values values = {
158 .little = latest->hand_curl[0],
159 .ring = latest->hand_curl[1],
160 .middle = latest->hand_curl[2],
161 .index = latest->hand_curl[3],
162 .thumb = latest->hand_curl[4],
163 };
164
165 // Get the pose of the hand.
166 struct xrt_space_relation relation;
167 xrt_result_t xret =
168 xrt_device_get_tracked_pose(xdev, XRT_INPUT_INDEX_GRIP_POSE, requested_timestamp_ns, &relation);
169 U_LOG_CHK_AND_RET(u_log_get_global_level(), xret, "xrt_device_get_tracked_pose");
170
171 // Simulate the hand.
172 enum xrt_hand hand = rd->is_left ? XRT_HAND_LEFT : XRT_HAND_RIGHT;
173 u_hand_sim_simulate_for_valve_index_knuckles(&values, hand, &relation, out_value);
174
175 out_value->is_active = latest->hand_tracking_active;
176
177 // This is a lie
178 *out_timestamp_ns = requested_timestamp_ns;
179 return XRT_SUCCESS;
180}
181
182/*!
183 * @public @memberof r_device
184 */
185struct xrt_device *
186r_device_create(struct r_hub *r, bool is_left)
187{
188 // Allocate.
189 const enum u_device_alloc_flags flags = 0;
190 const uint32_t input_count = 21; // 20 + hand tracker
191 const uint32_t output_count = 1;
192 struct r_device *rd = U_DEVICE_ALLOCATE( //
193 struct r_device, flags, input_count, output_count);
194
195 // Setup the basics.
196 rd->base.update_inputs = r_device_update_inputs;
197 rd->base.get_tracked_pose = r_device_get_tracked_pose;
198 rd->base.get_hand_tracking = r_device_get_hand_tracking;
199 rd->base.get_view_poses = u_device_ni_get_view_poses;
200 rd->base.set_output = u_device_ni_set_output;
201 rd->base.destroy = r_device_destroy;
202 rd->base.tracking_origin = &r->origin;
203 rd->base.supported.orientation_tracking = true;
204 rd->base.supported.position_tracking = true;
205 rd->base.supported.hand_tracking = true;
206 rd->base.name = XRT_DEVICE_INDEX_CONTROLLER;
207 rd->base.binding_profiles = vive_binding_profiles_index;
208 rd->base.binding_profile_count = vive_binding_profiles_index_count;
209 rd->r = r;
210 rd->is_left = is_left;
211
212 // Print name.
213 snprintf(rd->base.str, sizeof(rd->base.str), "Remote %s Controller", is_left ? "Left" : "Right");
214 snprintf(rd->base.serial, sizeof(rd->base.str), "Remote %s Controller", is_left ? "Left" : "Right");
215
216
217
218 // Inputs and outputs.
219 rd->base.inputs[0].name = XRT_INPUT_INDEX_SYSTEM_CLICK;
220 rd->base.inputs[1].name = XRT_INPUT_INDEX_SYSTEM_TOUCH;
221 rd->base.inputs[2].name = XRT_INPUT_INDEX_A_CLICK;
222 rd->base.inputs[3].name = XRT_INPUT_INDEX_A_TOUCH;
223 rd->base.inputs[4].name = XRT_INPUT_INDEX_B_CLICK;
224 rd->base.inputs[5].name = XRT_INPUT_INDEX_B_TOUCH;
225 rd->base.inputs[6].name = XRT_INPUT_INDEX_SQUEEZE_VALUE;
226 rd->base.inputs[7].name = XRT_INPUT_INDEX_SQUEEZE_FORCE;
227 rd->base.inputs[8].name = XRT_INPUT_INDEX_TRIGGER_CLICK;
228 rd->base.inputs[9].name = XRT_INPUT_INDEX_TRIGGER_VALUE;
229 rd->base.inputs[10].name = XRT_INPUT_INDEX_TRIGGER_TOUCH;
230 rd->base.inputs[11].name = XRT_INPUT_INDEX_THUMBSTICK;
231 rd->base.inputs[12].name = XRT_INPUT_INDEX_THUMBSTICK_CLICK;
232 rd->base.inputs[13].name = XRT_INPUT_INDEX_THUMBSTICK_TOUCH;
233 rd->base.inputs[14].name = XRT_INPUT_INDEX_TRACKPAD;
234 rd->base.inputs[15].name = XRT_INPUT_INDEX_TRACKPAD_FORCE;
235 rd->base.inputs[16].name = XRT_INPUT_INDEX_TRACKPAD_TOUCH;
236 rd->base.inputs[17].name = XRT_INPUT_INDEX_GRIP_POSE;
237 rd->base.inputs[18].name = XRT_INPUT_INDEX_AIM_POSE;
238 if (is_left) {
239 rd->base.inputs[19].name = XRT_INPUT_HT_CONFORMING_LEFT;
240 } else {
241 rd->base.inputs[19].name = XRT_INPUT_HT_CONFORMING_RIGHT;
242 }
243 rd->base.inputs[20].name = XRT_INPUT_GENERIC_PALM_POSE;
244
245 rd->base.outputs[0].name = XRT_OUTPUT_NAME_INDEX_HAPTIC;
246
247 if (is_left) {
248 rd->base.device_type = XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER;
249 } else {
250 rd->base.device_type = XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER;
251 }
252
253 // Setup variable tracker.
254 u_var_add_root(rd, rd->base.str, true);
255
256 return &rd->base;
257}