From 7e385aa810e8e3b118ee0d72b8a12292eb140bbe Mon Sep 17 00:00:00 2001 From: Moses Turner Date: Mon, 21 Jun 2021 12:28:01 -0500 Subject: [PATCH] d/ns: Big refactor --- src/xrt/drivers/CMakeLists.txt | 6 +- src/xrt/drivers/meson.build | 6 +- .../deformation_northstar.cpp | 13 +- .../deformation_northstar.h | 2 +- .../utility_northstar.h | 0 src/xrt/drivers/north_star/ns_hmd.c | 907 ++++++++++-------- src/xrt/drivers/north_star/ns_hmd.h | 62 +- 7 files changed, 539 insertions(+), 457 deletions(-) rename src/xrt/drivers/north_star/{distortion => distortion_3d}/deformation_northstar.cpp (97%) rename src/xrt/drivers/north_star/{distortion => distortion_3d}/deformation_northstar.h (98%) rename src/xrt/drivers/north_star/{distortion => distortion_3d}/utility_northstar.h (100%) diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index e43a7cb89..212f84317 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -113,9 +113,9 @@ endif() if(XRT_BUILD_DRIVER_NS) set(NS_SOURCE_FILES - north_star/distortion/utility_northstar.h - north_star/distortion/deformation_northstar.h - north_star/distortion/deformation_northstar.cpp + north_star/distortion_3d/utility_northstar.h + north_star/distortion_3d/deformation_northstar.h + north_star/distortion_3d/deformation_northstar.cpp north_star/ns_hmd.h north_star/ns_hmd.c north_star/ns_interface.h diff --git a/src/xrt/drivers/meson.build b/src/xrt/drivers/meson.build index 7e7c7ba49..ac0b12699 100644 --- a/src/xrt/drivers/meson.build +++ b/src/xrt/drivers/meson.build @@ -57,9 +57,9 @@ lib_drv_hydra = static_library( lib_drv_ns = static_library( 'drv_ns', files( - 'north_star/distortion/utility_northstar.h', - 'north_star/distortion/deformation_northstar.h', - 'north_star/distortion/deformation_northstar.cpp', + 'north_star/distortion_3d/utility_northstar.h', + 'north_star/distortion_3d/deformation_northstar.h', + 'north_star/distortion_3d/deformation_northstar.cpp', 'north_star/ns_hmd.h', 'north_star/ns_hmd.c', 'north_star/ns_interface.h', diff --git a/src/xrt/drivers/north_star/distortion/deformation_northstar.cpp b/src/xrt/drivers/north_star/distortion_3d/deformation_northstar.cpp similarity index 97% rename from src/xrt/drivers/north_star/distortion/deformation_northstar.cpp rename to src/xrt/drivers/north_star/distortion_3d/deformation_northstar.cpp index e816eedf9..a4c48bdae 100644 --- a/src/xrt/drivers/north_star/distortion/deformation_northstar.cpp +++ b/src/xrt/drivers/north_star/distortion_3d/deformation_northstar.cpp @@ -20,7 +20,7 @@ OpticalSystem::OpticalSystem(const OpticalSystem &_in) } void -OpticalSystem::LoadOpticalData(struct ns_v1_eye *eye) +OpticalSystem::LoadOpticalData(struct ns_3d_eye *eye) { ellipseMinorAxis = eye->ellipse_minor_axis; @@ -257,9 +257,8 @@ OpticalSystem::DisplayUVToRenderUVPreviousSeed(Vector2 inputUV) return curDisplayUV; } - extern "C" struct ns_optical_system * -ns_create_optical_system(struct ns_v1_eye *eye) +ns_3d_create_optical_system(struct ns_3d_eye *eye) { OpticalSystem *opticalSystem = new OpticalSystem(); opticalSystem->LoadOpticalData(eye); @@ -269,11 +268,11 @@ ns_create_optical_system(struct ns_v1_eye *eye) } extern "C" void -ns_display_uv_to_render_uv(struct ns_uv in, struct ns_uv *out, struct ns_v1_eye *eye) +ns_3d_display_uv_to_render_uv(struct xrt_vec2 in, struct xrt_vec2 *out, struct ns_3d_eye *eye) { OpticalSystem *opticalSystem = (OpticalSystem *)eye->optical_system; - Vector2 inUV = Vector2(in.u, 1.f - in.v); + Vector2 inUV = Vector2(in.x, 1.f - in.y); Vector2 outUV = opticalSystem->DisplayUVToRenderUVPreviousSeed(inUV); - out->u = outUV.x; - out->v = outUV.y; + out->x = outUV.x; + out->y = outUV.y; } diff --git a/src/xrt/drivers/north_star/distortion/deformation_northstar.h b/src/xrt/drivers/north_star/distortion_3d/deformation_northstar.h similarity index 98% rename from src/xrt/drivers/north_star/distortion/deformation_northstar.h rename to src/xrt/drivers/north_star/distortion_3d/deformation_northstar.h index dd646b3fd..5b24bb44f 100644 --- a/src/xrt/drivers/north_star/distortion/deformation_northstar.h +++ b/src/xrt/drivers/north_star/distortion_3d/deformation_northstar.h @@ -17,7 +17,7 @@ public: OpticalSystem(const OpticalSystem &_in); void - LoadOpticalData(struct ns_v1_eye *eye); + LoadOpticalData(struct ns_3d_eye *eye); Vector3 GetEyePosition() diff --git a/src/xrt/drivers/north_star/distortion/utility_northstar.h b/src/xrt/drivers/north_star/distortion_3d/utility_northstar.h similarity index 100% rename from src/xrt/drivers/north_star/distortion/utility_northstar.h rename to src/xrt/drivers/north_star/distortion_3d/utility_northstar.h diff --git a/src/xrt/drivers/north_star/ns_hmd.c b/src/xrt/drivers/north_star/ns_hmd.c index 3c9a88e18..58b29b61a 100644 --- a/src/xrt/drivers/north_star/ns_hmd.c +++ b/src/xrt/drivers/north_star/ns_hmd.c @@ -26,16 +26,397 @@ #include "util/u_debug.h" #include "util/u_device.h" #include "util/u_time.h" -#include "util/u_distortion_mesh.h" -#include "xrt/xrt_config_drivers.h" -#ifdef XRT_BUILD_DRIVER_RS -#include "../realsense/rs_interface.h" -#endif +#include "math/m_space.h" DEBUG_GET_ONCE_LOG_OPTION(ns_log, "NS_LOG", U_LOGGING_INFO) -struct xrt_pose t265_to_nose_bridge = XRT_POSE_IDENTITY; +#define printf_pose(pose) \ + printf("%f %f %f %f %f %f %f\n", pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, \ + pose.orientation.y, pose.orientation.z, pose.orientation.w); + + +static float +try_get_ipd(struct ns_hmd *ns, const struct cJSON *json) +{ // + const char *things[] = {"baseline", "ipd", "IPD"}; + bool done = false; + float out; + const char *thing; + for (int i = 0; (!done && (i < 3)); i++) { + thing = things[i]; + done = u_json_get_float(u_json_get(json, thing), &out); + } + if (!done) { + NS_INFO(ns, + "No key `baseline (or ipd, or IPD)` in your config file. Guessing the IPD is 64 millimeters"); + out = 64.0f; + } + if (out > 250.0f) { + NS_ERROR(ns, "IPD is way too high (%f millimeters!) Are you sure `%s` in your config file is correct?", + out, thing); + } + if (out < 10.0f) { + NS_ERROR(ns, "IPD is way too low (%f millimeters!) Are you sure `%s` in your config file is correct?", + out, thing); + } + out *= 0.001f; + NS_DEBUG(ns, "IPD returned is %f meters", out); + + return out; +} + +static void +try_get_fov(struct ns_hmd *ns, const struct cJSON *json, struct xrt_fov *left_fov, struct xrt_fov *right_fov) +{ + const char *things[] = {"fov", "FOV"}; + float out_float; + struct xrt_fov out_fov; + const char *thing; + for (int i = 0; (i < 2); i++) { + thing = things[i]; + const cJSON *fov_obj = u_json_get(json, thing); + if (fov_obj == NULL) { + continue; + } + if (u_json_get_float_array(fov_obj, &out_fov.angle_left, 4)) { // LRTB array of floats, this is allowed. + goto good; + } + if (u_json_get_float(fov_obj, &out_float)) { + out_fov.angle_left = -out_float; + out_fov.angle_right = out_float; + out_fov.angle_up = out_float; + out_fov.angle_down = -out_float; + goto good; + } + } + // Defaults, get skipped over if we found a FOV in the json + NS_INFO(ns, "No key `fov` in your config file. Guessing you want 0.7 radian half-angles."); + out_fov.angle_left = -0.7f; + out_fov.angle_right = 0.7f; + out_fov.angle_up = 0.7f; + out_fov.angle_down = -0.7f; + +good: + assert(out_fov.angle_right > out_fov.angle_left); + assert(out_fov.angle_up > out_fov.angle_down); + assert(fabsf(out_fov.angle_up) < M_PI_2); + assert(fabsf(out_fov.angle_down) < M_PI_2); + assert(fabsf(out_fov.angle_left) < M_PI_2); + assert(fabsf(out_fov.angle_right) < M_PI_2); + memcpy(left_fov, &out_fov, sizeof(struct xrt_fov)); + memcpy(right_fov, &out_fov, sizeof(struct xrt_fov)); + return; +} + +bool +ns_vipd_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) +{ + struct ns_hmd *ns = ns_hmd(xdev); + return u_compute_distortion_ns_vipd(&ns->dist_vipd, view, u, v, result); +} + +bool +ns_vipd_parse(struct ns_hmd *ns) +{ + + struct u_ns_vipd_values *temp_data = &ns->dist_vipd; + const struct cJSON *config_json = ns->config_json; + + const cJSON *grids_json = u_json_get(config_json, "grids"); + if (grids_json == NULL) + goto cleanup_vipd; + + const cJSON *current_element = NULL; + char *current_key = NULL; + + cJSON_ArrayForEach(current_element, grids_json) + { // Note to people reviewing this: this is definitely not super safe. Tried to add as many null-checks as + // possible etc. but is probably a waste of time, it takes a while to do this right and the only person using + // this code is me -Moses + current_key = current_element->string; + float ipd = strtof(current_key, NULL) / 1000; + if (!((ipd < .100) && (ipd > .030))) { + U_LOG_E("Nonsense IPD in grid %d, skipping", temp_data->number_of_ipds + 1); + continue; + } + + temp_data->number_of_ipds += 1; + temp_data->ipds = realloc(temp_data->ipds, temp_data->number_of_ipds * sizeof(float)); + temp_data->ipds[temp_data->number_of_ipds - 1] = ipd; + temp_data->grids = realloc(temp_data->grids, temp_data->number_of_ipds * sizeof(struct u_ns_vipd_grid)); + + for (int view = 0; view <= 1; view++) { + const struct cJSON *grid_root = u_json_get(current_element, view ? "right" : "left"); + // if view is 0, then left. if view is 1, then right + for (int lv = 0; lv < 65; lv++) { + struct cJSON *v_axis = cJSON_GetArrayItem(grid_root, lv); + + for (int lu = 0; lu < 65; lu++) { + struct cJSON *cell = cJSON_GetArrayItem(v_axis, lu + 1); + + struct cJSON *cellX = cJSON_GetArrayItem(cell, 0); + struct cJSON *cellY = cJSON_GetArrayItem(cell, 1); + if (grid_root == NULL || cell == NULL || v_axis == NULL || cellX == NULL || + cellY == NULL) { + NS_ERROR(ns, + "VIPD distortion config is malformed in some way, bailing."); + goto cleanup_vipd; + } + temp_data->grids[temp_data->number_of_ipds - 1].grid[view][lv][lu].x = + (float)cellX->valuedouble; + temp_data->grids[temp_data->number_of_ipds - 1].grid[view][lv][lu].y = + (float)cellY->valuedouble; + } + } + } + } + + float baseline = try_get_ipd(ns, config_json); + + struct u_ns_vipd_grid *high_grid; + struct u_ns_vipd_grid *low_grid; + float interp; + for (int i = 1; i < temp_data->number_of_ipds; i++) { + NS_DEBUG(ns, "looking at %f lower and %f upper\n", temp_data->ipds[i - 1], temp_data->ipds[i]); + if ((baseline >= temp_data->ipds[i - 1]) && (baseline <= temp_data->ipds[i])) { + NS_DEBUG(ns, "okay, IPD is between %f and %f\n", temp_data->ipds[i - 1], temp_data->ipds[i]); + high_grid = &temp_data->grids[i - 1]; + low_grid = &temp_data->grids[i]; + interp = math_map_ranges(baseline, temp_data->ipds[i - 1], temp_data->ipds[i], 0, 1); + NS_DEBUG(ns, "interp is %f\n", interp); + break; + } + } + + for (int view = 0; view <= 1; view++) { + for (int lv = 0; lv < 65; lv++) { + for (int lu = 0; lu < 65; lu++) { + temp_data->grid_for_use.grid[view][lv][lu].x = math_map_ranges( + interp, 0, 1, low_grid->grid[view][lv][lu].x, high_grid->grid[view][lv][lu].x); + temp_data->grid_for_use.grid[view][lv][lu].y = math_map_ranges( + interp, 0, 1, low_grid->grid[view][lv][lu].y, high_grid->grid[view][lv][lu].y); + } + } + } + + try_get_fov(ns, config_json, &temp_data->fov[0], &temp_data->fov[1]); + + // stupid + memcpy(&ns->base.hmd->views[0].fov, &temp_data->fov[0], sizeof(struct xrt_fov)); + memcpy(&ns->base.hmd->views[1].fov, &temp_data->fov[1], sizeof(struct xrt_fov)); + + printf("%f %f %f %f\n", ns->base.hmd->views[1].fov.angle_down, ns->base.hmd->views[1].fov.angle_left, + ns->base.hmd->views[1].fov.angle_right, ns->base.hmd->views[1].fov.angle_up); + + ns->head_pose_to_eye[0].orientation.x = 0.0f; + ns->head_pose_to_eye[0].orientation.y = 0.0f; + ns->head_pose_to_eye[0].orientation.z = 0.0f; + ns->head_pose_to_eye[0].orientation.w = 1.0f; + ns->head_pose_to_eye[0].position.x = -baseline / 2; + ns->head_pose_to_eye[0].position.y = 0.0f; + ns->head_pose_to_eye[0].position.z = 0.0f; + + + + ns->head_pose_to_eye[1].orientation.x = 0.0f; + ns->head_pose_to_eye[1].orientation.y = 0.0f; + ns->head_pose_to_eye[1].orientation.z = 0.0f; + ns->head_pose_to_eye[1].orientation.w = 1.0f; + ns->head_pose_to_eye[1].position.x = baseline / 2; + ns->head_pose_to_eye[1].position.y = 0.0f; + ns->head_pose_to_eye[1].position.z = 0.0f; + + ns->base.compute_distortion = &ns_vipd_mesh_calc; + + return true; + +cleanup_vipd: + memset(&ns->dist_vipd, 0, sizeof(struct u_ns_vipd_values)); + return false; +} + +/* + * + * "2D Polynomial" distortion; original implementation by Johnathon Zelstadt + * Sometimes known as "v2", filename is often NorthStarCalibration.json + * + */ + +static bool +ns_p2d_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) +{ + struct ns_hmd *ns = ns_hmd(xdev); + return u_compute_distortion_ns_p2d(&ns->dist_p2d, view, u, v, result); +} + + +bool +ns_p2d_parse(struct ns_hmd *ns) +{ + + struct xrt_pose temp_eyes_center_to_eye[2]; + + // convenience names + const struct cJSON *config_json = ns->config_json; + + // Note that x and y are flipped. We have to flip 'em at some point - the polynomial calibrator has a strange + // definition of x and y. "opencv treats column major over row major (as in, Y,X for image look up)" -Dr. Damo + if (u_json_get_float_array(u_json_get(config_json, "left_uv_to_rect_x"), ns->dist_p2d.y_coefficients_right, + 16) != 16) + goto cleanup_p2d; + if (u_json_get_float_array(u_json_get(config_json, "left_uv_to_rect_y"), ns->dist_p2d.x_coefficients_right, + 16) != 16) + goto cleanup_p2d; + if (u_json_get_float_array(u_json_get(config_json, "right_uv_to_rect_x"), ns->dist_p2d.y_coefficients_left, + 16) != 16) + goto cleanup_p2d; + if (u_json_get_float_array(u_json_get(config_json, "right_uv_to_rect_y"), ns->dist_p2d.x_coefficients_left, + 16) != 16) + goto cleanup_p2d; + + // at this point, locked into using this distortion method - we can touch anything and not worry about side + // effects + float baseline = try_get_ipd(ns, config_json); + + math_pose_identity(&temp_eyes_center_to_eye[0]); + math_pose_identity(&temp_eyes_center_to_eye[1]); + temp_eyes_center_to_eye[0].position.x = -baseline / 2; + temp_eyes_center_to_eye[1].position.x = baseline / 2; + + try_get_fov(ns, config_json, &ns->dist_p2d.fov[0], &ns->dist_p2d.fov[1]); + + memcpy(&ns->base.hmd->views[0].fov, &ns->dist_p2d.fov[0], sizeof(struct xrt_fov)); + memcpy(&ns->base.hmd->views[1].fov, &ns->dist_p2d.fov[1], sizeof(struct xrt_fov)); + + ns->base.compute_distortion = &ns_p2d_mesh_calc; + memcpy(&ns->head_pose_to_eye, &temp_eyes_center_to_eye, sizeof(struct xrt_pose) * 2); + + return true; + +cleanup_p2d: + memset(&ns->dist_p2d, 0, sizeof(struct u_ns_p2d_values)); + return false; +} + + +/* + * + * "Original 3D" undistortion, by Leap Motion + * Sometimes known as "v1", config file name is often "Calibration.json" + * + */ + +static bool +ns_3d_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) +{ + struct ns_hmd *ns = ns_hmd(xdev); + struct ns_3d_data *data = &ns->dist_3d; + struct xrt_vec2 uv = {u, v}; + struct xrt_vec2 warped_uv = {0.0f, 0.0f}; + + ns_3d_display_uv_to_render_uv(uv, &warped_uv, &data->eyes[view]); + + result->r.x = warped_uv.x; + result->r.y = warped_uv.y; + result->g.x = warped_uv.x; + result->g.y = warped_uv.y; + result->b.x = warped_uv.x; + result->b.y = warped_uv.y; + return true; +} + +static void +ns_3d_fov_calculate(struct xrt_fov *fov, struct xrt_quat projection) +{ // All of these are wrong - gets "hidden" by the simple_fov making it look + // okay. Needs to be fixed. + + fov->angle_up = projection.x; // atanf(fabsf(projection.x) / + // near_plane); + fov->angle_down = projection.y; // atanf(fabsf(projection.y) / near_plane); + fov->angle_left = projection.z; // atanf(fabsf(projection.z) / near_plane); + fov->angle_right = projection.w; // atanf(fabsf(projection.w) / near_plane); +} + +/* + * + * Parse functions. + * + */ + +static bool +ns_3d_leap_parse(struct ns_3d_leap *leap, const struct cJSON *leap_data) +{ + u_json_get_string_into_array(u_json_get(leap_data, "name"), leap->name, 64); + u_json_get_string_into_array(u_json_get(leap_data, "serial"), leap->serial, 64); + if (!u_json_get_vec3(u_json_get(u_json_get(leap_data, "localPose"), "position"), &leap->pose.position)) + return false; + if (!u_json_get_quat(u_json_get(u_json_get(leap_data, "localPose"), "rotation"), &leap->pose.orientation)) + return false; + return true; +} + +static bool +ns_3d_eye_parse(struct ns_3d_eye *eye, const struct cJSON *eye_data) +{ + if (!u_json_get_float(u_json_get(eye_data, "ellipseMinorAxis"), &eye->ellipse_minor_axis)) + return false; + if (!u_json_get_float(u_json_get(eye_data, "ellipseMajorAxis"), &eye->ellipse_major_axis)) + return false; + if (!u_json_get_vec3(u_json_get(eye_data, "screenForward"), &eye->screen_forward)) + return false; + if (!u_json_get_vec3(u_json_get(eye_data, "screenPosition"), &eye->screen_position)) + return false; + if (!u_json_get_vec3(u_json_get(eye_data, "eyePosition"), &eye->eye_pose.position)) + return false; + if (!u_json_get_quat(u_json_get(eye_data, "eyeRotation"), &eye->eye_pose.orientation)) + return false; + if (!u_json_get_quat(u_json_get(eye_data, "cameraProjection"), &eye->camera_projection)) + return false; + for (int x = 0; x < 4; ++x) { + for (int y = 0; y < 4; ++y) { + char key[4]; + sprintf(key, "e%d%d", x, y); + + u_json_get_float(u_json_get(u_json_get(eye_data, "sphereToWorldSpace"), key), + &eye->sphere_to_world_space.v[(x * 4) + y]); + u_json_get_float(u_json_get(u_json_get(eye_data, "worldToScreenSpace"), key), + &eye->world_to_screen_space.v[(x * 4) + y]); + } + } + return true; +} + +bool +ns_3d_parse(struct ns_hmd *ns) +{ + struct ns_3d_data *our_ns_3d_data = &ns->dist_3d; + + if (!ns_3d_eye_parse(&our_ns_3d_data->eyes[0], u_json_get(ns->config_json, "leftEye"))) + goto cleanup_l3d; + if (!ns_3d_eye_parse(&our_ns_3d_data->eyes[1], u_json_get(ns->config_json, "rightEye"))) + goto cleanup_l3d; + if (!ns_3d_leap_parse(&our_ns_3d_data->leap, u_json_get(ns->config_json, "leapTracker"))) + goto cleanup_l3d; + + // Locked in, okay to touch anything inside ns struct + ns_3d_fov_calculate(&ns->base.hmd->views[0].fov, our_ns_3d_data->eyes[0].camera_projection); + ns_3d_fov_calculate(&ns->base.hmd->views[1].fov, our_ns_3d_data->eyes[1].camera_projection); + + ns->head_pose_to_eye[0] = our_ns_3d_data->eyes[0].eye_pose; // Left eye. + ns->head_pose_to_eye[1] = our_ns_3d_data->eyes[1].eye_pose; // Right eye. + + our_ns_3d_data->eyes[0].optical_system = ns_3d_create_optical_system(&our_ns_3d_data->eyes[0]); + our_ns_3d_data->eyes[1].optical_system = ns_3d_create_optical_system(&our_ns_3d_data->eyes[1]); + + ns->base.compute_distortion = &ns_3d_mesh_calc; + + return true; + +cleanup_l3d: + memset(&ns->dist_3d, 0, sizeof(struct ns_3d_data)); + return false; +} /* * @@ -43,14 +424,6 @@ struct xrt_pose t265_to_nose_bridge = XRT_POSE_IDENTITY; * */ -static double -map(double value, double fromLow, double fromHigh, double toLow, double toHigh) -{ - return (value - fromLow) * (toHigh - toLow) / (fromHigh - fromLow) + toLow; -} // Math copied from - // https://www.arduino.cc/reference/en/language/functions/math/map/ -// This is pure math so it is still under the Boost Software License. - static void ns_hmd_destroy(struct xrt_device *xdev) { @@ -66,12 +439,6 @@ static void ns_hmd_update_inputs(struct xrt_device *xdev) {} -/* - * - * V1 functions - * - */ - static void ns_hmd_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, @@ -85,10 +452,7 @@ ns_hmd_get_tracked_pose(struct xrt_device *xdev, return; } - out_relation->pose = ns->pose; - out_relation->relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | - XRT_SPACE_RELATION_POSITION_VALID_BIT | - XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); + *out_relation = ns->no_tracker_relation; // you can change this using the debug gui } static void @@ -98,350 +462,71 @@ ns_hmd_get_view_pose(struct xrt_device *xdev, struct xrt_pose *out_pose) { struct ns_hmd *ns = ns_hmd(xdev); - *out_pose = ns->eye_configs_v1[view_index].eye_pose; + *out_pose = ns->head_pose_to_eye[view_index]; } - -/* - * - * V1 Mesh functions. - * - */ - static bool -ns_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) -{ - struct ns_hmd *ns = ns_hmd(xdev); - - struct ns_uv uv = {u, v}; - struct ns_uv warped_uv = {0.0f, 0.0f}; - ns_display_uv_to_render_uv(uv, &warped_uv, &ns->eye_configs_v1[view]); - - result->r.x = warped_uv.u; - result->r.y = warped_uv.v; - result->g.x = warped_uv.u; - result->g.y = warped_uv.v; - result->b.x = warped_uv.u; - result->b.y = warped_uv.v; - return true; -} - -/* - * - * Parse function. - * - */ - -static void -ns_leap_parse(struct ns_leap *leap, struct cJSON *leap_data) -{ - /* - These are very wrong! - You could very likely write into random memory here. - - u_json_get_string(cJSON_GetObjectItemCaseSensitive(leap_data, - "name"), &leap->name); - u_json_get_string(cJSON_GetObjectItemCaseSensitive(leap_data, - "serial"), &leap->serial); - */ - - u_json_get_vec3( - cJSON_GetObjectItemCaseSensitive(cJSON_GetObjectItemCaseSensitive(leap_data, "localPose"), "position"), - &leap->pose.position); - u_json_get_quat( - cJSON_GetObjectItemCaseSensitive(cJSON_GetObjectItemCaseSensitive(leap_data, "localPose"), "rotation"), - &leap->pose.orientation); -} - -static void -ns_eye_parse(struct ns_v1_eye *eye, struct cJSON *eye_data) -{ - u_json_get_float(cJSON_GetObjectItemCaseSensitive(eye_data, "ellipseMinorAxis"), &eye->ellipse_minor_axis); - u_json_get_float(cJSON_GetObjectItemCaseSensitive(eye_data, "ellipseMajorAxis"), &eye->ellipse_major_axis); - u_json_get_vec3(cJSON_GetObjectItemCaseSensitive(eye_data, "screenForward"), &eye->screen_forward); - u_json_get_vec3(cJSON_GetObjectItemCaseSensitive(eye_data, "screenPosition"), &eye->screen_position); - u_json_get_vec3(cJSON_GetObjectItemCaseSensitive(eye_data, "eyePosition"), &eye->eye_pose.position); - u_json_get_quat(cJSON_GetObjectItemCaseSensitive(eye_data, "eyeRotation"), &eye->eye_pose.orientation); - u_json_get_quat(cJSON_GetObjectItemCaseSensitive(eye_data, "cameraProjection"), &eye->camera_projection); - for (int x = 0; x < 4; ++x) { - for (int y = 0; y < 4; ++y) { - char key[4]; - sprintf(key, "e%d%d", x, y); - - u_json_get_float(cJSON_GetObjectItemCaseSensitive( - cJSON_GetObjectItemCaseSensitive(eye_data, "sphereToWorldSpace"), key), - &eye->sphere_to_world_space.v[(x * 4) + y]); - u_json_get_float(cJSON_GetObjectItemCaseSensitive( - cJSON_GetObjectItemCaseSensitive(eye_data, "worldToScreenSpace"), key), - &eye->world_to_screen_space.v[(x * 4) + y]); - } - } -} - - -static void -ns_fov_calculate(struct xrt_fov *fov, struct xrt_quat projection) -{ // All of these are wrong - gets "hidden" by the simple_fov making it look - // okay. Needs to be fixed. - - fov->angle_up = projection.x; // atanf(fabsf(projection.x) / - // near_plane); - fov->angle_down = projection.y; // atanf(fabsf(projection.y) / near_plane); - fov->angle_left = projection.z; // atanf(fabsf(projection.z) / near_plane); - fov->angle_right = projection.w; // atanf(fabsf(projection.w) / near_plane); -} - -/* - * - * V2 optics. - * - * - */ - - -static void -ns_v2_hmd_get_view_pose(struct xrt_device *xdev, - const struct xrt_vec3 *eye_relation, - uint32_t view_index, - struct xrt_pose *out_pose) -{ - struct ns_hmd *ns = ns_hmd(xdev); - struct xrt_vec3 real_eye_relation = *eye_relation; - real_eye_relation.x = ns->ipd; - - u_device_get_view_pose(&real_eye_relation, view_index, out_pose); -} - -static float -ns_v2_polyval2d(float X, float Y, float C[16]) -{ - float X2 = X * X; - float X3 = X2 * X; - float Y2 = Y * Y; - float Y3 = Y2 * Y; - return (((C[0]) + (C[1] * Y) + (C[2] * Y2) + (C[3] * Y3)) + - ((C[4] * X) + (C[5] * X * Y) + (C[6] * X * Y2) + (C[7] * X * Y3)) + - ((C[8] * X2) + (C[9] * X2 * Y) + (C[10] * X2 * Y2) + (C[11] * X2 * Y3)) + - ((C[12] * X3) + (C[13] * X3 * Y) + (C[14] * X3 * Y2) + (C[15] * X3 * Y3))); -} - - - -static void -ns_v2_fov_calculate(struct ns_hmd *ns, int eye_index) -{ - ns->base.hmd->views[eye_index].fov.angle_down = ns->eye_configs_v2[eye_index].fov.angle_down; - ns->base.hmd->views[eye_index].fov.angle_up = ns->eye_configs_v2[eye_index].fov.angle_up; - ns->base.hmd->views[eye_index].fov.angle_left = ns->eye_configs_v2[eye_index].fov.angle_left; - ns->base.hmd->views[eye_index].fov.angle_right = ns->eye_configs_v2[eye_index].fov.angle_right; -} - - - -static bool -ns_v2_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) -{ - /*! @todo (Moses Turner) It should not be necessary to reverse the U and - * V coords. I have no idea why it is like this. It shouldn't be like - * this. It must be something wrong with the undistortion calibrator. - * The V2 undistortion calibrator software is here if you want to look: - * https://github.com/BryanChrisBrown/ProjectNorthStar/tree/feat-gen-2-software/Software/North%20Star%20Gen%202/North%20Star%20Calibrator - */ - // u = 1.0 - u; - v = 1.0 - v; - - - struct ns_hmd *ns = ns_hmd(xdev); - - float x_ray = ns_v2_polyval2d(u, v, ns->eye_configs_v2[view].x_coefficients); - float y_ray = ns_v2_polyval2d(u, v, ns->eye_configs_v2[view].y_coefficients); - - - float left_ray_bound = tan(ns->eye_configs_v2[view].fov.angle_left); - float right_ray_bound = tan(ns->eye_configs_v2[view].fov.angle_right); - float up_ray_bound = tan(ns->eye_configs_v2[view].fov.angle_up); - float down_ray_bound = tan(ns->eye_configs_v2[view].fov.angle_down); - - float u_eye = map(x_ray, left_ray_bound, right_ray_bound, 0, 1); - - float v_eye = map(y_ray, down_ray_bound, up_ray_bound, 0, 1); - - - // boilerplate, put the UV coordinates in all the RGB slots - result->r.x = u_eye; - result->r.y = v_eye; - result->g.x = u_eye; - result->g.y = v_eye; - result->b.x = u_eye; - result->b.y = v_eye; - - return true; -} - - -static bool -ns_config_load(struct ns_hmd *ns) +ns_config_load(struct ns_hmd *ns, const char *config_path) { // Get the path to the JSON file - if (ns->config_path == NULL || strcmp(ns->config_path, "/") == 0) { - NS_ERROR(ns, - "Configuration path \"%s\" does not lead to a " - "configuration JSON file. Set the NS_CONFIG_PATH env " - "variable to your JSON.", - ns->config_path); + bool json_allocated = false; + if (config_path == NULL || strcmp(config_path, "/") == 0) { + NS_INFO(ns, + "Configuration path \"%s\" does not lead to a " + "configuration JSON file. Set the NS_CONFIG_PATH env " + "variable to your JSON.", + config_path); return false; } - // Open the JSON file and put its contents into a string - FILE *config_file = fopen(ns->config_path, "r"); + FILE *config_file = fopen(config_path, "r"); if (config_file == NULL) { - NS_ERROR(ns, "The configuration file at path \"%s\" was unable to load", ns->config_path); - return false; + NS_INFO(ns, "The configuration file at path \"%s\" was unable to load", config_path); + goto parse_error; } - char json[8192]; + fseek(config_file, 0, SEEK_END); // Go to end of file + int file_size = ftell(config_file); // See offset we're at. This should be the file size in bytes. + rewind(config_file); // Go back to the beginning of the file + + if (file_size == 0) { + NS_INFO(ns, "Empty config file!"); + goto parse_error; + } else if (file_size > 3 * pow(1024, 2)) { // 3 MiB + NS_INFO(ns, "Huge config file! (%f MiB!!) Something's wrong here.", ((float)file_size) / pow(1024, 2)); + goto parse_error; + } + + char *json = malloc(file_size + 1); + json_allocated = true; + size_t i = 0; - while (!feof(config_file) && i < (sizeof(json) - 1)) { + while (!feof(config_file)) { json[i++] = fgetc(config_file); } json[i] = '\0'; - struct cJSON *config_json; - - // Parse the JSON file - config_json = cJSON_Parse(json); - if (config_json == NULL) { + ns->config_json = cJSON_Parse(json); + if (ns->config_json == NULL) { const char *error_ptr = cJSON_GetErrorPtr(); - NS_ERROR(ns, "The JSON file at path \"%s\" was unable to parse", ns->config_path); + NS_INFO(ns, "The JSON file at path \"%s\" was unable to parse", config_path); if (error_ptr != NULL) { - NS_ERROR(ns, "because of an error before %s", error_ptr); + NS_INFO(ns, "because of an error before %s", error_ptr); } - return false; - } - if (cJSON_GetObjectItemCaseSensitive(config_json, "leftEye") == NULL && - cJSON_GetObjectItemCaseSensitive(config_json, "left_uv_to_rect_x") != NULL) { - // Bad hack to tell that we're v2. Error checking is not good - // enough for public consumption - many cases where a malformed - // config json results in cryptic errors. Should get fixed - // whenever we have more than 5 people using this. - u_json_get_float(cJSON_GetObjectItemCaseSensitive(config_json, "baseline"), &ns->ipd); - ns->ipd = ns->ipd / 1000.0f; // converts from mm to m - /*! @todo (Moses Turner) Next four u_json_get_float_array calls - * don't make any sense. They put the X coefficients from the - * JSON file into the Y coefficients in the structs, which is - * totally wrong, but the distortion looks totally wrong if we - * don't do this. - */ - u_json_get_float_array(cJSON_GetObjectItemCaseSensitive(config_json, "left_uv_to_rect_x"), - ns->eye_configs_v2[0].y_coefficients, 16); - u_json_get_float_array(cJSON_GetObjectItemCaseSensitive(config_json, "left_uv_to_rect_y"), - ns->eye_configs_v2[0].x_coefficients, 16); - u_json_get_float_array(cJSON_GetObjectItemCaseSensitive(config_json, "right_uv_to_rect_x"), - ns->eye_configs_v2[1].y_coefficients, 16); - u_json_get_float_array(cJSON_GetObjectItemCaseSensitive(config_json, "right_uv_to_rect_y"), - ns->eye_configs_v2[1].x_coefficients, 16); - bool said_first_thing = false; - if (!u_json_get_float(cJSON_GetObjectItemCaseSensitive(config_json, "left_fov_radians_left"), - &ns->eye_configs_v2[0].fov.angle_left)) { // not putting this directly in - // (&ns->base.hmd->views[eye_index].fov - // because i smell a rat there - - // that value seems to unexpectedly - // change during init process. - NS_INFO(ns, - "Just so you know, you can add tunable FoV parameters to your v2 json file. There are " - "examples in src/xrt/drivers/north_star/exampleconfigs.\n"); - said_first_thing = true; - ns->eye_configs_v2[0].fov.angle_left = -0.8; - ns->eye_configs_v2[0].fov.angle_right = 0.8; - ns->eye_configs_v2[0].fov.angle_up = 0.8; - ns->eye_configs_v2[0].fov.angle_down = -0.8; - - ns->eye_configs_v2[1].fov.angle_left = -0.8; - ns->eye_configs_v2[1].fov.angle_right = 0.8; - ns->eye_configs_v2[1].fov.angle_up = 0.8; - ns->eye_configs_v2[1].fov.angle_down = -0.8; - - - - } else { - - /*! @todo (Moses Turner) Something's wrong with either - * ns_v2_mesh_calc or this code, because when you have - * uneven FOV bounds, the distortion looks totally - * wrong.*/ - u_json_get_float(cJSON_GetObjectItemCaseSensitive(config_json, "left_fov_radians_left"), - &ns->eye_configs_v2[0].fov.angle_left); - u_json_get_float(cJSON_GetObjectItemCaseSensitive(config_json, "left_fov_radians_right"), - &ns->eye_configs_v2[0].fov.angle_right); - u_json_get_float(cJSON_GetObjectItemCaseSensitive(config_json, "left_fov_radians_up"), - &ns->eye_configs_v2[0].fov.angle_up); - u_json_get_float(cJSON_GetObjectItemCaseSensitive(config_json, "left_fov_radians_down"), - &ns->eye_configs_v2[0].fov.angle_down); - - u_json_get_float(cJSON_GetObjectItemCaseSensitive(config_json, "right_fov_radians_left"), - &ns->eye_configs_v2[1].fov.angle_left); - u_json_get_float(cJSON_GetObjectItemCaseSensitive(config_json, "right_fov_radians_right"), - &ns->eye_configs_v2[1].fov.angle_right); - u_json_get_float(cJSON_GetObjectItemCaseSensitive(config_json, "right_fov_radians_up"), - &ns->eye_configs_v2[1].fov.angle_up); - u_json_get_float(cJSON_GetObjectItemCaseSensitive(config_json, "right_fov_radians_down"), - &ns->eye_configs_v2[1].fov.angle_down); - } - - struct cJSON *offset = cJSON_GetObjectItemCaseSensitive(config_json, "t265_to_nose_bridge"); - if (offset == NULL) { - if (said_first_thing) { - NS_INFO(ns, - "Also, you should put an offset parameter into the json file to transform your " - "head pose from the realsense to your nose bridge. There are some examples in " - "src/xrt/drivers/north_star/exampleconfigs/"); - } else { - NS_INFO(ns, - "You should put an offset parameter into the json file to transform your head " - "pose from the realsense to your nose bridge. There are some examples in " - "src/xrt/drivers/north_star/exampleconfigs/."); - } - } else { - struct cJSON *translation_meters = - cJSON_GetObjectItemCaseSensitive(offset, "translation_meters"); - u_json_get_float(cJSON_GetObjectItemCaseSensitive(translation_meters, "x"), - &t265_to_nose_bridge.position.x); - u_json_get_float(cJSON_GetObjectItemCaseSensitive(translation_meters, "y"), - &t265_to_nose_bridge.position.y); - u_json_get_float(cJSON_GetObjectItemCaseSensitive(translation_meters, "z"), - &t265_to_nose_bridge.position.z); - - struct cJSON *rotation_quaternion = - cJSON_GetObjectItemCaseSensitive(offset, "rotation_quaternion"); - - u_json_get_float(cJSON_GetObjectItemCaseSensitive(rotation_quaternion, "x"), - &t265_to_nose_bridge.orientation.x); - u_json_get_float(cJSON_GetObjectItemCaseSensitive(rotation_quaternion, "y"), - &t265_to_nose_bridge.orientation.y); - u_json_get_float(cJSON_GetObjectItemCaseSensitive(rotation_quaternion, "z"), - &t265_to_nose_bridge.orientation.z); - u_json_get_float(cJSON_GetObjectItemCaseSensitive(rotation_quaternion, "w"), - &t265_to_nose_bridge.orientation.w); - } - - ns->is_v2 = true; - - - } else if (cJSON_GetObjectItemCaseSensitive(config_json, "leftEye") != NULL && - cJSON_GetObjectItemCaseSensitive(config_json, "left_uv_to_rect_x") == NULL) { - ns_eye_parse(&ns->eye_configs_v1[0], cJSON_GetObjectItemCaseSensitive(config_json, "leftEye")); - - ns_eye_parse(&ns->eye_configs_v1[1], cJSON_GetObjectItemCaseSensitive(config_json, "rightEye")); - ns_leap_parse(&ns->leap_config, cJSON_GetObjectItemCaseSensitive(config_json, "leapTracker")); - ns->is_v2 = false; - } else { - NS_ERROR(ns, - "Bad config file. There are examples of v1 and v2 files in " - "src/xrt/drivers/north_star/exampleconfigs - if those don't work, something's really wrong."); + goto parse_error; } - cJSON_Delete(config_json); + // this function is not supposed to return true if ns->config_json is NULL + assert(ns->config_json != NULL); + free(json); return true; + +parse_error: + if (json_allocated) { + free(json); + } + NS_INFO(ns, "Are you sure you're using the right configuration file?"); + return false; } @@ -456,17 +541,47 @@ ns_hmd_create(const char *config_path) { enum u_device_alloc_flags flags = (enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE); - - struct ns_hmd *ns = U_DEVICE_ALLOCATE(struct ns_hmd, flags, 1, 0); + ns->ll = debug_get_log_option_ns_log(); + + if (!ns_config_load(ns, config_path)) + goto cleanup; // don't need to print any error, ns_config_load did that for us + + int number_wrap = 3; // number of elements in below array of function pointers. Const to stop compiler warnings. + bool (*wrap_func_ptr[3])(struct ns_hmd *) = {ns_3d_parse, ns_p2d_parse, ns_vipd_parse}; + // C syntax is weird here. This is an array of pointers to functions with arguments (struct ns_system * system) + // that all return a boolean value. The array should be roughly in descending order of how likely we think the + // user means to use each method For now VIPD is last because Moses is the only one that uses it + + bool found_config_wrap = false; + for (int i = 0; i < number_wrap; i++) { + if (wrap_func_ptr[i](ns)) { // wrap_func_ptr[i](ns) is a function call! + U_LOG_I("North Star: Using config wrap %i", i); + found_config_wrap = true; + break; + } + } // This will segfault at function ?? if you use GDB and the length is wrong. + + if (!found_config_wrap) { + NS_INFO(ns, "North Star: Config file seems to be invalid."); + goto cleanup; + } + ns->base.update_inputs = ns_hmd_update_inputs; ns->base.get_tracked_pose = ns_hmd_get_tracked_pose; - // NOT HERE ns->base.get_view_pose = ns_hmd_get_view_pose; + ns->base.get_view_pose = ns_hmd_get_view_pose; ns->base.destroy = ns_hmd_destroy; ns->base.name = XRT_DEVICE_GENERIC_HMD; - ns->pose.orientation.w = 1.0f; // All other values set to zero. - ns->config_path = config_path; - ns->ll = debug_get_log_option_ns_log(); + math_pose_identity(&ns->no_tracker_relation.pose); + ns->no_tracker_relation.relation_flags = (enum xrt_space_relation_flags)( + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | + XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); + // Appeases the inner workings of Monado for when there's no head tracker and we're giving a fake pose through + // the debug gui + ns->base.orientation_tracking_supported = true; + ns->base.position_tracking_supported = true; + ns->base.device_type = XRT_DEVICE_TYPE_HMD; + // Print name. snprintf(ns->base.str, XRT_DEVICE_NAME_LEN, "North Star"); @@ -474,81 +589,55 @@ ns_hmd_create(const char *config_path) // Setup input. ns->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE; - // Setup info. - struct u_device_simple_info info; - info.display.w_pixels = 2880; - info.display.h_pixels = 1600; - info.display.w_meters = 0.13f; - info.display.h_meters = 0.07f; - info.lens_horizontal_separation_meters = 0.13f / 2.0f; - info.lens_vertical_position_meters = 0.07f / 2.0f; - info.views[0].fov = 70.0f * (M_PI / 180.0f); - info.views[1].fov = 70.0f * (M_PI / 180.0f); + struct u_extents_2d exts; + // info.w_meters = 0.0588f * 2.0f; + // info.h_meters = 0.0655f; - if (!ns_config_load(ns)) - goto cleanup; + // one NS screen is 1440px wide, but there are two of them. + exts.w_pixels = 1440 * 2; + // Both NS screens are 1600px tall + exts.h_pixels = 1600; - // ns_config_load() sets ns->is_v2. - // to change how we switch between v1 and v2, - // you'll need to start in ns_config_load() - if (ns->is_v2) { - ns->base.get_view_pose = ns_v2_hmd_get_view_pose; - ns_v2_fov_calculate(ns, 0); - ns_v2_fov_calculate(ns, 1); - // Setup the north star basic info - if (!u_device_setup_split_side_by_side(&ns->base, &info)) { - NS_ERROR(ns, "Failed to setup basic device info"); - goto cleanup; - } - ns_v2_fov_calculate(ns, 0); - ns_v2_fov_calculate(ns, 1); + u_extents_2d_split_side_by_side(&ns->base, &exts); - ns->base.hmd->distortion.models = XRT_DISTORTION_MODEL_COMPUTE; - ns->base.hmd->distortion.preferred = XRT_DISTORTION_MODEL_COMPUTE; - ns->base.compute_distortion = ns_v2_mesh_calc; + ns->base.hmd->distortion.models = XRT_DISTORTION_MODEL_COMPUTE; + ns->base.hmd->distortion.preferred = XRT_DISTORTION_MODEL_COMPUTE; - } else { - // V1 - ns->base.get_view_pose = ns_hmd_get_view_pose; - ns_fov_calculate(&ns->base.hmd->views[0].fov, ns->eye_configs_v1[0].camera_projection); - ns_fov_calculate(&ns->base.hmd->views[1].fov, ns->eye_configs_v1[1].camera_projection); - - // Create the optical systems - ns->eye_configs_v1[0].optical_system = ns_create_optical_system(&ns->eye_configs_v1[0]); - ns->eye_configs_v1[1].optical_system = ns_create_optical_system(&ns->eye_configs_v1[1]); - - // Setup the north star basic info - if (!u_device_setup_split_side_by_side(&ns->base, &info)) { - NS_ERROR(ns, "Failed to setup basic device info"); - goto cleanup; - } - - ns->base.hmd->distortion.models = XRT_DISTORTION_MODEL_COMPUTE; - ns->base.hmd->distortion.preferred = XRT_DISTORTION_MODEL_COMPUTE; - ns->base.compute_distortion = ns_mesh_calc; - } + ns->base.get_view_pose = ns_hmd_get_view_pose; // Setup variable tracker. u_var_add_root(ns, "North Star", true); - u_var_add_pose(ns, &ns->pose, "pose"); + u_var_add_pose(ns, &ns->no_tracker_relation.pose, "pose"); ns->base.orientation_tracking_supported = true; ns->base.device_type = XRT_DEVICE_TYPE_HMD; size_t idx = 0; - // Preferred; most North Stars are see-through. + // Preferred; almost all North Stars (as of early 2021) are see-through. ns->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_ADDITIVE; - // XRT_BLEND_MODE_OPAQUE is not preferred and kind of a lie, but we use North Star for VR sometimes despite its - // see-through display. And there's nothing stopping you from covering up the outside of the reflector, turning - // it into an opaque headset. As most VR apps I've encountered require BLEND_MODE_OPAQUE to be an option, we - // need to support it. + // XRT_BLEND_MODE_OPAQUE is not preferred and kind of a lie, but you can totally use North Star for VR apps, + // despite its see-through display. And there's nothing stopping you from covering up the outside of the + // reflector, turning it into an opaque headset. As most VR apps I've encountered require BLEND_MODE_OPAQUE to + // be an option, we need to support it. ns->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE; - // Not supporting ALPHA_BLEND for now, because I know nothing about it and want to avoid unintended - // consequences. As soon as you have a specific reason to support it, go ahead and support it. + // Not supporting ALPHA_BLEND for now, because I know nothing about it, have no reason to use it, and want to + // avoid unintended consequences. As soon as you have a specific reason to support it, go ahead and support it. ns->base.hmd->num_blend_modes = idx; + uint64_t start, end; + + start = os_monotonic_get_ns(); + u_distortion_mesh_fill_in_compute(&ns->base); + end = os_monotonic_get_ns(); + + float diff = (end - start); + diff /= U_TIME_1MS_IN_NS; + + NS_DEBUG(ns, "Filling mesh took %f ms", diff); + + return &ns->base; cleanup: diff --git a/src/xrt/drivers/north_star/ns_hmd.h b/src/xrt/drivers/north_star/ns_hmd.h index 5b1909bda..d9e09932d 100644 --- a/src/xrt/drivers/north_star/ns_hmd.h +++ b/src/xrt/drivers/north_star/ns_hmd.h @@ -1,9 +1,12 @@ // Copyright 2019, Collabora, Ltd. +// Copyright 2020, Nova King. +// Copyright 2021, Moses Turner. // SPDX-License-Identifier: BSL-1.0 /*! * @file * @brief Interface between North Star distortion and HMD code. * @author Nova King + * @author Moses Turner * @ingroup drv_ns */ @@ -15,6 +18,10 @@ #include "xrt/xrt_defines.h" #include "xrt/xrt_device.h" #include "util/u_logging.h" +#include "os/os_threading.h" +#include "util/u_distortion_mesh.h" + +#include #ifdef __cplusplus extern "C" { @@ -34,7 +41,8 @@ extern "C" { /* * - * Structs + * 3D distortion structs + * Sometimes known as "v1", config file name is often "Calibration.json" * */ @@ -43,18 +51,7 @@ extern "C" { * * @ingroup drv_ns */ -struct ns_optical_system; - -/*! - * Simple UV struct. - * - * @ingroup drv_ns - */ -struct ns_uv -{ - float u; - float v; -}; +struct ns_3d_optical_system; /*! * Configuration information about the LMC or Rigel sensor according to the @@ -62,10 +59,10 @@ struct ns_uv * * @ingroup drv_ns */ -struct ns_leap +struct ns_3d_leap { - const char *name; - const char *serial; + char name[64]; + char serial[64]; struct xrt_pose pose; }; @@ -74,7 +71,7 @@ struct ns_leap * * @ingroup drv_ns */ -struct ns_v1_eye +struct ns_3d_eye { float ellipse_minor_axis; float ellipse_major_axis; @@ -92,12 +89,10 @@ struct ns_v1_eye struct ns_optical_system *optical_system; }; -struct ns_v2_eye +struct ns_3d_data { - float x_coefficients[16]; - float y_coefficients[16]; - struct xrt_pose eye_pose; - struct xrt_fov fov; + struct ns_3d_eye eyes[2]; + struct ns_3d_leap leap; }; /*! @@ -109,21 +104,19 @@ struct ns_v2_eye struct ns_hmd { - struct xrt_device base; - struct xrt_pose pose; - + struct xrt_space_relation no_tracker_relation; const char *config_path; + struct cJSON *config_json; + struct xrt_pose head_pose_to_eye[2]; // left, right - struct ns_v1_eye eye_configs_v1[2]; // will be NULL if is_v2. - struct ns_v2_eye eye_configs_v2[2]; // will be NULL if !is_v2 - float ipd; - - struct ns_leap leap_config; // will be NULL if is_v2 + union { + struct ns_3d_data dist_3d; + struct u_ns_p2d_values dist_p2d; + struct u_ns_vipd_values dist_vipd; + }; enum u_logging_level ll; - bool is_v2; // True if V2, false if V1. If we ever get a v3 this should - // be an enum or something }; /* @@ -148,11 +141,12 @@ ns_hmd(struct xrt_device *xdev) * * @ingroup drv_ns */ + void -ns_display_uv_to_render_uv(struct ns_uv in, struct ns_uv *out, struct ns_v1_eye *eye); +ns_3d_display_uv_to_render_uv(struct xrt_vec2 in, struct xrt_vec2 *out, struct ns_3d_eye *eye); struct ns_optical_system * -ns_create_optical_system(struct ns_v1_eye *eye); +ns_3d_create_optical_system(struct ns_3d_eye *eye); #ifdef __cplusplus